基于图优化理论的大规模三维环境地图创建方法

摘要

本发明提供一种基于图优化理论的大规模三维环境地图创建方法。该方法首先利用裁剪迭代最近点算法顺序地估计移动机器人的局部位姿，并判断估计结果的可靠性，如果结果不可靠，则采用快速的基于多尺度描述子对应传播算法重新估计当前时刻移动机器人的局部位姿，同时逐渐地构造一个位姿图，图的顶点表示移动机器人各时刻的位姿，图的边表示相连位姿之间的约束；随后提出了一种闭环假设和验证方法，用来检测位姿图中的闭环；最后通过运动平均方法求解位姿约束方程，得到精确的移动机器人的全局位姿。实验结果表明该方法可以很好地创建大规模三维环境地图。

说明书

技术领域

本发明涉及移动机器人、导航定位和计算机视觉领域，具体涉及一种利用裁剪迭代最近点算法TrICP和快速的基于多尺度描述子对应传播算法顺序地估计移动机器人的局部位姿、利用自适应闭环假设和验证方法检测位姿图中的闭环、利用运动平均方法计算精确的移动机器人的全局位姿的方法。

背景技术

随着计算机和传感器设备的迅速发展，移动机器人技术已经广泛地应用于人类生产和生活相关的各个领域。移动机器人在自主执行各种任务的过程中，需要获得可靠的位姿信息，而位姿信息的获取依赖于精确的环境地图。为此，除了安装各种执行机构外，移动机器人上还必须安装大量的例如激光雷达、声纳、红外相机和视觉相机等感知设备。这些安装了多种传感器的移动机器人可用于采集环境感知信息，然后借助于同步定位与地图创建(Simultaneous Localization and Mapping，SLAM)技术创建合适的环境地图并实现其自身的可靠定位，以保证移动机器人顺利执行各种任务。目前，很多自主移动机器人均具备SLAM功能。这些自主移动机器人已在各个领域得到广泛应用，主要包括：无人驾驶汽车，农林业，服务业、军事和矿产资源开采等领域。下面将介绍SLAM技术在上述几个领域中的应用：

1)无人驾驶汽车

无人驾驶车是当今人工智能领域的热点问题，它在国防和智能辅助安全驾驶等领域有着广泛应用，研究具有人工智能的无人驾驶平台可减少战场上和交通事故中的人员伤亡。与普通车相比，无人驾驶车增加了诸如激光雷达，毫米波雷达，视觉相机和红外相机等传感器。它在行驶的过程中需要借助于这些传感器采集车体周围环境的感知信息，然后利用相关的SLAM算法和技术将感知信息转化成计算机能够理解的环境地图，并提供给规划模块进行运动规划，以获得合适的控制输入量驱动无人驾驶车向前行驶。现有无人驾驶车定位严重的依赖于GPS信号,在无GPS信号的环境下，SLAM技术的重要性将显得更加突出。

2)矿产资源开采

矿产资源是人类生产和生活的必需品，因此如何合理地开采和利用矿产资源是人类面临的一大挑战。在中国，澳大利亚和美国等资源大国，存在着数以万计的矿井，能否绘制精确的矿井地图，对于资源的合理开采和矿难救援均可起到关键的作用。而采用人工绘制矿井地图的方式即费时又无法保证精确性和可靠性，对于一些存在安全隐患的废弃矿井，人工根本无法绘制出可靠和精确的矿井地图。目前，国内矿难事故频发，救援人员在救援过程中急需精确的矿井地图。为此，可在矿车上安装计算平台、激光雷达传感器和视频采集设备，然后利用SLAM算法和技术自主地绘制出二维或三维矿井地图。与人工绘制方式相比，利用移动机器人绘制地图的方式更加可靠且精确，目前SLAM技术已成功应用于矿井地图的绘制工作中。但对于规模较大的矿井，其地图创建仍然是一个有待解决的问题。

3)农林业种植与防护

树木覆盖着全球大部分面积并且对二氧化碳下降、动物群落、水文湍流调节和巩固土壤起着重要作用，是构成地球生物圈当中的一个最重要方面。但木材又是工业、农业和建筑等行业的重要原材料，因此如何高效地种植树木和有效地利用树木也是一个重要的问题。为了保证树木的快速生长，需要合理控制树木的密度；为了有效地利用树木资源，需要了解树木的大小以及已成材树木的位置。虽然人工也能完成上述工作，但成本较高且精度较低，而利用安装了激光雷达和视觉传感器的移动机器人，通过设计合理的运动路径，并借助于SLAM算法和技术，即可快速自主地获得精确的林区树木分布地图。该地图中除了包含树木的位置信息外，还可包含树木的大小信息，这些结果可用于控制林区树木的种植密度，以便高效地种植树木和有效地利用木材。目前加拿大、芬兰等国家的研究者已将SLAM技术用于辅助工人进行林业种植与防护，此外SLAM技术已逐渐在农业种植中得到应用，但大多只能在平地上实现同步定位与地图创建。

4)家庭和社会服务

人口老龄化问题是全球面临的一个重要问题。为解决人口老龄化等所导致的家庭和社会服务问题，研发出具有高性价比的家庭和社会服务机器人已成为了许多研究机构和公司的关注点，这些服务机器人可替代人工执行自主吸尘，搬运物件和导航服务等功能。而执行上述任务一个最基本的前提是服务机器人能实现自身的精确定位和导航，一种简单可靠的方法是在服务机器人上安装一些传感器，并按一定的规则在室内运动并采集环境数据，然后利用SLAM算法和技术建立一幅完整的室内环境地图。在执行日常的服务时，可采用SLAM算法实现服务机器人的精确定位以及动态地修改环境地图。但对于包含较多运动物体的环境，家庭和社会服务移动机器人的SLAM算法需要进一步提高可靠性和稳定性。

5)特殊环境中的探索

地球上资源大部分为不可再生资源。随着人类文明的进步，人类可开采的资源已日益枯竭，这就促使人类探索特殊环境中的资源。在火山口、深海、北极、其它行星以及受污染的环境中可能蕴藏着各种丰富的资源，但受空气、压力和温度等因素的限制，人类目前还无法到达或长期驻扎在这些特殊环境中，因而限制了人类在这些环境中的活动。而移动机器人可以适应各种特殊的环境，因此研制具有勘探特殊环境功能的移动机器人是人工智能领域的一大热点。显然，在勘探的过程中需要对移动机器人进行可靠定位，而可靠的定位必须依赖于精确的环境地图，这些功能均可由安装了特殊传感器的移动机器人借助于SLAM技术实现。

总之，SLAM技术已经成为人工智能和移动机器人研究领域中及其重要且极富挑战性的课题。它不仅在陆地移动机器人上取得的广泛的应用，而且在水下移动机器人和空中移动机器人上也逐渐得到了应用。其目的是为了创建环境地图并实现移动机器人的精确定位，确保移动机器人可顺利地执行各项任务。虽然相关文献已给出了各种有效的解决方法，但大部分方法只适用于解决中小规模环境下的地图创建问题。而实际应用中，经常需要解决大规模环境下的地图创建问题。与中小规模环境下的地图创建问题，大规模环境下的地图创建问题更具复杂性。

发明内容

本发明的目的在于克服现有的三维环境地图创建方法的缺点，提供一种基于图优化理论的大规模三维环境地图创建方法。

为达到上述目的，本发明采用了以下技术方案。

一种基于图优化理论的大规模三维环境地图创建方法，包括以下步骤：

1)获取移动机器人的局部位姿

首先利用移动机器人搭载的激光扫描仪扫描周围环境获取三维点云，同时利用裁剪迭代最近点算法TrICP对相邻时刻采集的两帧三维点云进行配准，该算法的初始值为上一个时刻移动机器人的位姿变化值；获得配准结果后，需要对配准结果进行校验；如校验成功则将配准结果作为移动机器人的位姿估计结果输出，如校验失败，则利用快速的基于多尺度描述子对应传播算法重新估计移动机器人的局部位姿，该快速的基于多尺度描述子对应传播算法是对已有的基于多尺度描述子对应传播算法进行了加速，具体来说，是先对已经建立的种子匹配对根据在描述符D上的距离大小以降序方式进行了排序，然后只对前δ×100％(δ＝0.3)的种子匹配对应用对应传播机制；

通过结合裁剪迭代最近点算法和快速的基于多尺度描述子对应传播算法，得到移动机器人的局部位姿，随后利用下面的公式获取移动机器人的初始全局定位结果：

其中M₁为4×4的单位矩阵,M_i+1和M_i分别表示移动机器人在i+1和i时刻的全局位姿,表示移动机器人从i时刻到i+1时刻的局部位姿；基于移动机器人的初始全局定位结果，逐渐地构建一个位姿图，图的顶点表示移动机器人各时刻的位姿，图的边表示相连位姿之间的约束；

2)闭环检测

首先计算当前时刻移动机器人全局位姿与之前时刻全局位姿的欧式距离，假如该距离小于移动机器人在当前时刻自适应阈值，则假设检测到了闭环，然后利用快速的基于多尺度描述子对应传播算法对闭环假设所涉及当前和先前时刻所采集的两帧点云数据进行双视角全局配准，获得配准结果并判断相应的裁剪均方误差：如果裁剪均方误差小于β,则接受闭环假设；否则拒绝闭环假设；如果接受了当前的闭环假设，则设置这个环的权重为1/k，k表示检测到的闭环为第k个闭环。

3)获取精确的移动机器人全局位姿

根据移动机器人的初始全局定位结果和闭环检测结果建立移动机器人相邻时刻和不同时刻的位姿约束方程，利用运动平均算法求解移动机器人相邻时刻和不同时刻的位姿约束方程，获取精确的移动机器人全局位姿；基于精确的移动机器人全局位姿，将各时刻获得的三维点云变换到全局坐标系下，进而得到精确的大规模三维环境地图。

步骤1)所述的对配准结果进行校验的公式为：

其中，θ_x表示当前时刻与上一个时刻的俯仰角变化值，θ_y表示当前时刻与上一个时刻的偏航角变化值，θ_z表示当前时刻与上一个时刻的翻滚角变化值,TMSE表示裁剪迭代最近点算法的裁剪均方误差，α和β表示设定的阈值，a取值为0.35，β取值为0.02。

步骤2)所述的计算当前时刻移动机器人全局位姿与之前时刻全局位姿的欧式距离的公式为：

d_ij＝||t_i-t_j||₂

其中t_i和t_j分别表示移动机器人在i时刻和j时刻的全局位置。

步骤2)所述的移动机器人在当前时刻自适应阈值的计算公式为：

其中c一个预设的参数，取值为0.1；d_i.i+1表示移动机器人在i时刻和i+1时刻全局位姿的欧氏距离,s为闭环假设的起始时刻，j为当前时刻。

步骤3)所述的建立的移动机器人相邻时刻的位姿约束方程为：

其中M_i+1和M_i分别表示移动机器人在i+1和i时刻的全局位姿,表示移动机器人从i时刻到i+1时刻的局部位姿。

步骤3)所述的建立的移动机器人不同时刻的位姿约束方程为：

其中M_i和M_j分别表示移动机器人在i和j时刻的全局位姿,表示移动机器人从i时刻到j时刻的局部位姿。

本发明首先通过集成裁剪迭代最近点算法和快速的基于多尺度描述子对应传播算法顺序地估计移动机器人的局部位姿，同时利用局部位姿递推地计算移动机器人的初始全局位姿并构造位姿图，图的顶点表示移动机器人各时刻的位姿，图的边表示相连位姿之间的约束；随后利用自适应闭环假设和验证方法检测位姿图中的闭环；最后通过运动平均方法求解位姿约束方程，得到精确的移动机器人的全局位姿。实验结果表明该方法可以很好地创建大规模三维环境地图。

本发明有以下优点：

1)将裁剪迭代最近点算法和快速的基于多尺度描述子对应传播算法结合起来估计移动机器人的局部位姿，保证了三维地图创建的效率和可靠性；

2)采用自适应的闭环假设和验证方法，能够有效地检测位姿图中的闭环；

3)利用运动平均方法优化已经建立的位姿图，可以得到精确的移动机器人全局位姿。

附图说明

图1是本发明的流程示意图。

图2是快速的基于多尺度描述子对应传播算法的原理图。

图3是闭环检测的示意图。

图4是建立约束方程，用运动平均算法优化的示意图。

图5是本发明与其他两种算法在Honnover2数据集上三维地图对比图，其中，图5(a)只采用裁剪迭代最近点算法的结果；图5(b)只采用裁剪迭代最近点算法和快速的基于多尺度描述子对应传播算法；图5(c)为本发明的结果。

具体实施方式

下面结合附图对本发明作进一步说明。

参见图1，基于图优化理论的大规模三维环境地图创建方法分为三部分，各个部分包括的步骤如下：

1)由裁剪迭代最近点算法和快速的基于多尺度描述子对应传播算法得到局部位姿，具体步骤如下：

(1a)通过裁剪迭代最近点算法对相邻时刻采集获得的两帧三维点云进行配准；

(1b)获得配准结果后，对配准结果进行校验；如校验成功则可将配准结果作为移动机器人的位姿估计结果输出；

(1c)如校验失败，则利用快速的基于多尺度描述子对应传播算法重新估计移动机器人的局部位姿；

图2为快速的基于多尺度描述子对应传播算法的原理图。首先要对移动机器人采集到的扫描数据进行降采样，接着计算多尺度描述符(N,D)，其中N表示基于法线的描述符，D表示基于特征值的描述符。在得到三维扫描数据中每个点的多尺度描述符之后，可以对多尺度描述符D应用最近邻搜索，从而建立种子匹配对。接着对这些种子匹配对根据点对距离已降序的方式进行排序，然后对前δ×100％(δ＝0.3)的应用对应传播机制。每个扩展后的种子匹配都可以利用RANSAC算法计算出移动机器人的局部位姿，根据裁剪均方误差的大小，可以找到最优的局部位姿M_best。最后，可以用裁剪迭代最近点算法对得到的局部位姿进行进一步优化。

2)闭环检测，具体步骤如下：

(2a)首先计算当前时刻移动机器人全局位姿与之前时刻全局位姿的欧式距离；

(2b)假如该距离小于在当前时刻自适应阈值，则假设检测到了闭环；

(2c)然后利用快速的基于多尺度描述子对应传播算法对检测到的闭环进行验证。

图3为闭环检测的原理图。当前时刻移动机器人全局位姿与之前时刻全局位姿的欧式距离的计算公式为：

d_ij＝||t_i-t_j||₂

其中t_i和t_j分别表示移动机器人在i时刻和j时刻的全局位置。

由于存在累计误差问题，距离阈值大小应该与路径长度呈正相关，因此移动机器人在i时刻自适应阈值的计算公式为：

其中c是一个预设的参数，取值为0.1；d_i.i+1表示移动机器人在i时刻和i+1时刻全局位姿的欧氏距离,s为闭环假设的起始时刻，j为当前时刻。如果d_ij≤r>

3)建立位姿约束方程，求解精确的移动机器人全局位姿，图4为建立位姿约束方程的示意图。具体步骤如下：

(3a)建立相邻时刻的位姿约束方程为：

其中M_i+1和M_i分别表示移动机器人在i+1和i时刻的全局位姿,表示移动机器人从i时刻到i+1时刻的局部位姿。

(3b)建立不同时刻的位姿约束方程为：

其中M_i和M_j分别表示移动机器人在i+1和j时刻的全局位姿,表示移动机器人从i时刻到j时刻的局部位姿。

(3c)利用运动平均算法求解位姿约束方程，获得精确的移动机器人全局位姿。

本发明首先通过集成裁剪迭代最近点算法和快速的基于多尺度描述子对应传播算法顺序地估计移动机器人的局部位姿，提高了局部定位系统的精度、效率和鲁棒性；同时利用局部位姿递推地计算移动机器人的初始全局位姿并构造位姿图，图的顶点表示移动机器人各时刻的位姿，图的边表示相连位姿之间的约束；随后利用自适应闭环假设和验证方法检测位姿图中的闭环；最后通过运动平均方法求解位姿约束方程，得到精确的移动机器人的全局位姿。实验结果表明该方法可以很好地创建大规模三维环境地图。

根据图5，可以看出本发明在实际应用中的效果。该数据移动机器人在大学校园环境中的三维环境地图创建结果，移动机器人的行车路长度为1.24km,采集到924帧扫描数据，每帧扫描数据大约包含16600个点。图5(a)仅仅采用裁剪迭代最近点算法得到的结果，可以看到裁剪迭代最近点算法无法准确定位移动机器人的位置。图5(b)运动平均算法优化之前的结果，可以看到仅仅采用裁剪迭代最近点算法和快速的基于多尺度描述子对应传播算法会产生累计误差，无法得到精确的三维环境地图；图5(c)为本发明中的算法，可以看到本发明中的算法可以创建精确的三维环境地图。实验结果表明本发明可以为大规模环境有效地创建精确的三维环境地图。