CN105096386B - 大范围复杂城市环境几何地图自动生成方法 - Google Patents

Abstract

一种大范围复杂城市环境几何地图生成方法。其包括提取二维道路及二维建筑物轮廓信息，生成二维几何地图；由二维几何地图引导车载单目云台摄像机进行视频采集，由此获得视频图像；从视频图像中提取出关键帧；提取与匹配相邻关键帧之间的线段特征，由此得到线段对应；基于匹配的线特征的移动机器人同时定位与环境三维几何模型构建；将三维几何模型与二维几何地图进行匹配，确定二者对应关系，生成城市增强型几何地图；基于带几何约束的Bundle Adjustment算法对城市增强型几何地图进行优化等步骤。本发明与现有技术相比，具有适用场景范围大、鲁棒性高、精度高、所需存储空间小、成本低等优点。

Description

大范围复杂城市环境几何地图自动生成方法

技术领域

本发明属于移动机器人技术领域，尤其是一种面向移动机器人自主导航的大范围复杂城市环境几何地图自动生成方法。

背景技术

环境地图生成是实现移动机器人自主导航任务的基础和关键。而环境地图的表示形式以及地图构建方法则是移动机器人领域研究的热点和难点问题。

随着智能辅助驾驶以及无人驾驶车辆等技术的发展，城市环境中的移动机器人自主导航日益受到重视，随之而来大范围城市环境地图生成问题成为研究热点。

现有用于移动机器人在大范围城市环境中导航的地图仍然以二维的道路以及建筑物轮廓信息为主，这些信息可以从电子地图或卫星地图数据库中获取。然而，二维地图只能提供给机器人二维环境信息，无法用于移动机器人(如低空小型无人机)的三维定位及导航应用。为此，一些学者开始关注构建城市环境的三维地图模型。然而，目前用于三维城市环境地图表示的视觉特征大多为初级特征，即基于重建的点特征以及线段特征等。但由于初级视觉特征存在诸如精度低、计算量大、鲁棒性差等问题，特别是在城市环境中，虽然视觉特征非常丰富但相似的场景及特征也非常多，因此视觉特征经常受到来自光线、阴影等因素的干扰。此外，现有的三维城市模型尚未将二维道路及建筑物轮廓信息与三维视觉特征结合，各种特征之间的几何关系和约束并没有充分利用，致使地图生成过程中各特征之间相互独立，从而影响了环境地图构建的准确性和鲁棒性。Microsoft Visual Earth和Google Earth是两项比较成功的对城市环境进行三维建模的应用，然而，由于它们都需要进行全尺度的三维重建，所以算法的时间复杂度和空间复杂度都非常高，致使其并不适合诸如小型无人机等资源受限的移动平台的应用。

同时定位与建图(Simultaneously Localization and Mapping,SLAM)技术是实现移动机器人环境地图构建的重要理论框架。在SLAM中，周围环境被表示为“路标”的集合。比如，当利用激光测距仪或者深度相机作为主要传感器时，“路标”即为周围环境中的点云。在视觉SLAM中，“路标”通常为SIFT点特征或者线特征。在SLAM中，“路标”可以看作是一种初级的环境地图表示方式。越来越多的学者意识到了路标的选取对环境地图生成的重要影响。低层次路标相对容易提取，如Harris角点、SIFT特征点等，但点特征容易受到光照和阴影等自然因素的影响，计算量大且精度不高。为此，一些学者以线段和边缘等作为路标。最近，开始有学者将点、线段和平面等的组合作为路标。如Gee等将三维平面和直线组合用于视觉SLAM中；Martinez等通过提取点特征和平面实现了单目视觉SLAM。Cham等利用在全景图像中提取竖直的建筑物边缘以及该边缘两侧的建筑物平面的方向来估计摄像机的位姿。Delmerico等利用立体视觉对空间点进行重建，然后基于RANSAC方法来对点进行采样和分组，最终确定一系列的候选建筑物平面。这些方法说明越来越多的学者关注到了高层次路标在环境地图表示中的重要性。然而，现有工作都是仅仅将不同特征作为孤立的几何物体来看待，但并未充分挖掘出不同特征内部的联系，而且没有充分发挥高层次路标的作用，因而对于环境地图构建的精度有限，鲁棒性也不高。因此，亟需一种更高层次的、强鲁棒性、高精度的环境地图表示与生成方法。

目前，人造场景中的移动机器人SLAM方法主要应用于室内环境，并已经取得了***的研究成果。与室内环境相比，城市环境是一个典型的大范围、复杂场景。因此，城市环境中的机器人定位与环境地图构建方法设计面临着更大的挑战。虽然也有学者尝试进行室外环境下的大范围SLAM，但在计算效率和精度方面仍不够理想。在大范围环境中，现有SLAM算法对时间、空间需求的矛盾更加突出。对于室外或者复杂环境，由于传感器感知的不确定性更加明显，不确定性传递造成的***误差更加严重，现有SLAM算法很难取得很好的效果。因此，目前SLAM算法的计算量大、实时性差、对环境的鲁棒性不高等缺点使其在大型或室外环境中的应用受到限制。

综上所述，现有环境地图生成方法主要存在以下问题：1.环境地图构成的元素仍以点云或线段为主，缺少对更高层次特征的提取，算法复杂度较高且鲁棒性较差；2.缺少对于不同层次特征之间几何关系的分析及充分利用，各种特征仍被视为孤立的个体，致使地图表示精度以及算法鲁棒性均不高；3.仅限于小范围或室内环境使用，由于算法计算量大、实时性差、对环境的鲁棒性不高，所以很难在大范围复杂城市环境中应用。

发明内容

为了解决上述问题，本发明的目的在于提供一种能够为大范围城市环境中移动机器人自主导航应用提供技术基础的大范围复杂城市环境几何地图自动生成方法。

为了达到上述目的，本发明提供的大范围复杂城市环境几何地图自动生成方法包括按顺序进行的下列步骤：

(1)提取二维道路及二维建筑物轮廓信息，生成二维几何地图；

(2)由二维几何地图引导车载单目云台摄像机进行视频采集，由此获得视频图像；

(3)从步骤(2)中得到的视频图像中提取出关键帧；

(4)提取与匹配相邻关键帧之间的线段特征，由此得到线段对应；

(5)基于匹配的线段特征的移动机器人同时定位与环境三维几何模型构建；

(6)将三维几何模型与二维几何地图进行匹配，确定二者对应关系，生成城市增强型几何地图；

(7)基于带几何约束的Bundle Adjustment算法对上述城市增强型几何地图进行优化；

在步骤(5)中，所述的基于匹配的线段特征的移动机器人同时定位与环境三维几何模型构建的方法是：共包括初始估计和优化估计两个阶段；在初始估计阶段，利用获得的相邻关键帧间的匹配线段，基于RANSAC算法估计出基本矩阵F，然后利用摄像机内参数矩阵和基本矩阵F估计出本质矩阵E，之后通过分解本质矩阵E估计出移动机器人的位姿变化，最后利用三角法重构得到三维空间中的线段，并基于共面线段估计出相应的竖直平面；在优化估计阶段，利用各种特征之间的几何约束，通过构造并求解优化函数完成对初始估计结果的优化，以降低估计误差随机器人运动范围的扩大而不断累积。

在步骤(7)中，所述的基于带几何约束的Bundle Adjustment算法对上述城市增强型几何地图进行优化的方法是：首先，基于城市增强型几何地图中各种特征之间的几何约束，分别构造线段重投影误差、共面约束代价函数、可见约束代价函数；然后，基于局部Bundle Adjustment算法对几何地图进行优化：利用k时刻之前N个关键帧来对m个最近的机器人位姿和k-m+1时刻后新建的环境地图进行重新调整和优化；优化过程的代价函数定义为构造的几何约束代价函数的加权和，利用Levenberg-Marquardt算法对该优化问题进行求解。

本发明提供的大范围复杂城市环境几何地图自动生成方法是首先定义了综合了二维及三维几何特征的增强型几何地图EGMap，然后基于几何约束和优化算法实现对EGMap的估计。

本发明与现有技术相比，具有适用场景范围大、鲁棒性高、精度高、所需存储空间小、成本低等优点。本方法可供各类移动机器人在城市环境中的自主三维定位及导航应用。本发明的优点和积极效果具体如下：

1.本大范围复杂城市环境几何地图自动生成方法在二维道路地图及建筑物轮廓信息的基础上，基于车载单目摄像机建立周围环境的三维几何模型，并建立了二维几何地图与三维几何模型之间的对应关系，从而使生成的地图既能满足地面移动车辆二维定位与导航的需要，又能满足低空小型无人机的三维自主导航应用。

2.本大范围复杂城市环境几何地图自动生成方法利用不同层次特征之间的几何关系和约束，生成三维水平线段、竖直线段、竖直平面等高层次路标，并设计带几何约束的局部Bundle Adjustment算法对环境地图进行优化，从而使地图生成方法的鲁棒性更强、准确性更高，用于存储环境地图所需的空间更小，有利于后续移动机器人精确、实时定位和导航应用。

3.本大范围复杂城市环境几何地图自动生成方法基于匹配的线特征进行竖直平面的重建，并以线特征和竖直平面作为三维几何地图构成元素，与现有方法相比，本算法的鲁棒性更高，时间复杂度和空间复杂度也更低，使其适用于大范围环境地图的构建，解决了现有SLAM方法仅能用于室内或小范围环境内使用的问题。

4.本大范围复杂城市环境几何地图自动生成方法仅利用一台车载云台单目摄像机，成本低且配置方便，解决了现有基于激光扫描仪或双目视觉***等高复杂性、高成本的问题，同时便于***功能的扩展。

附图说明

图1是EGMap示意图；

图2是EGMap中不同层次几何特征之间的相互关系；

图3是本发明提供的大范围复杂城市环境几何地图自动生成方法流程图；

图4是本发明提供的大范围复杂城市环境几何地图自动生成方法中线段特征提取与匹配流程图；

图5是实验平台及实验场景。

具体实施方式

下面参照附图和具体实施例对本发明提供的大范围复杂城市环境几何地图自动生成方法进行详细说明。

本发明涉及的大范围城市环境几何地图命名为增强型几何地图(EnhancedGeometric Map，EGMap)。EGMap的示意图如图1所示。EGMap中包含有二维道路地图、二维建筑物俯视轮廓信息以及高层次三维几何模型(水平直线、竖直直线以及竖直建筑物平面)。EGMap中不同层次特征之间的几何关系如图2所示，其中，二维建筑物俯视轮廓是三维竖直平面(建筑物侧平面)的垂直投影；二维道路模型为三维道路模型的垂直投影；三维道路模型与三维竖直平面之间存在的可见关系是指移动机器人位于该三维道路上时，可以通过车载摄像机获取对应三维竖直平面的图像；如果一组三维线段位于同一个竖直平面上，则它们符合共面关系。

如图3所示，本发明提供的大范围复杂城市环境几何地图自动生成方法包括按顺序进行的下列步骤：

(1)提取二维道路及二维建筑物轮廓信息，生成二维几何地图；

通过公开的地理信息***数据库提供的应用程序接口读取二维道路信息；从高分辨率的卫星地图中提取出二维建筑物俯视轮廓信息，具体方法是：从公开的地理信息***数据库中获取移动机器人工作区域的卫星地图，针对卫星地图中建筑物以及非建筑物区域的特点，同时借助相应的普通城市地图，首先提取卫星地图中的线段特征，并通过对线段主方向分析而获得候选的建筑物边缘；然后利用区域分割、区域分析以及区域合并的方法确定出候选的建筑物轮廓；最后借助建筑物轮廓的先验几何知识，对候选的建筑物轮廓进行检验和确认，从而完成二维建筑物俯视轮廓信息的自动提取。由此生成二维几何地图。

(2)由二维几何地图引导车载单目云台摄像机进行视频采集，由此获得视频图像；

利用车载单目云台摄像机采集周围建筑物视频，根据移动机器人与步骤(1)中得到的二维几何地图中建筑物轮廓的相对位置关系，自动调整云台摄像机的朝向，以使云台摄像机采集的视频中包含周围的主要建筑物。

(3)从步骤(2)中得到的视频图像中提取出关键帧；

视频图像中的一幅图像帧被选中为关键帧需要满足以下两个条件：1)与上一关键帧之间摄像机位姿变化较大；2)与上一关键帧具有明显的重叠区域。具体来讲，首先将上述视频图像的第一帧图像选定为关键帧I₀；然后，假设I_k-1为上一关键帧，则将当前图像I_k作为关键帧的充要条件是I_k与I_k-1之间SIFT特征点匹配的数量大于某一设定的阈值N_s。

(4)提取与匹配相邻关键帧之间的线段特征，由此得到线段对应；

如图4所示，提取与匹配相邻关键帧之间的线段特征的具体步骤如下：

1)SIFT提取与匹配：对相邻关键帧中的点特征通过SIFT算法进行提取并匹配；

2)线段提取：采用LSD算法对每幅关键帧中的线段进行提取；

3)线段初始匹配：对帧间的线段特征进行初始匹配：为每条线段建立一个矩形邻域，对于任意两条线段，如果位于二者邻域内的已匹配的SIFT点的个数大于阈值N，则认为这两条线段是一对匹配线段；通过该步骤，并不能保证所有匹配的线段对应都正确，而且仍然存在一些线段对应没有被找到。

4)线段匹配修正：基于共面关系对上述初始匹配结果进行确认和修正。基于竖直平面诱导的单应矩阵模型，利用RANSAC算法对初始匹配的线段进行确认和分组，并借助这些单应矩阵模型找到更多在初始匹配阶段未被发现的线段对应；

最终，获得了两帧之间的线段匹配结果，同时按照线段所位于的竖直平面的不同对匹配结果进行了分组，即共面的线段被分为了一组。

(5)基于匹配的线段特征的移动机器人同时定位与环境三维几何模型构建；

机器人同时定位与周围环境三维几何模型构建主要包括两个环节：初始估计阶段、优化估计阶段。

初始估计阶段，利用获得的相邻关键帧间的线段对应，基于RANSAC算法估计出基本矩阵F，进而利用式(1)计算出本质矩阵E，其中K为摄像机内参数矩阵，可以通过标定事先获得。

E＝K^TFK (1)

通过分解本质矩阵E，可以获得作为移动机器人的摄像机当前相对于上一关键帧时刻的平移向量t_k和旋转矩阵R_k，即获得了移动机器人的位姿信息。

在获得了移动机器人的位姿后，基于图像帧匹配的线段对应，利用三角法可以重构得到三维空间中的线段集合L_k，然后利用一组共面线段，估计得到三维空间中线段所在竖直平面集合Π_k，从而完成了几何地图的初始估计。以上过程生成的环境地图和机器人位姿估计均包含较大误差，该误差会随着机器人运动范围的扩大而不断累积、放大，因此，需要对该初始估计结果进行优化。

在优化估计阶段，将各种特征之间的几何约束考虑其中，通过求解如下优化问题模型来优化初始估计阶段获得的三维几何特征：

其中，各项代价函数分别表示线段的重投影误差、线特征方向约束、平面的方向约束、线段的共面约束、可见约束以及二维道路模型约束。λ＝[λ₁,λ₂,λ₃,λ₄,λ₅,λ₆]^T分别表示各代价函数的权重系数。下面分别对各代价函数进行定义。

1)线段的重投影误差：重投影误差定义为：

满足：

其中，和是二维线段l_k的两个端点，和是重建得到的三维线段l的重投影的两个端点，d_⊥()表示点到直线的距离，d()表示两点之间的距离。式(3)的前4项为直线的重投影误差，后4项为线段端点的重投影误差。

2)线段的方向约束：三维空间中的一组平行线交于一无穷远点，称为消失点。重建时，该组平行线中的任一线段都应通过该消失点，即线段的方向都受到相应消失点的约束。

其中，O_i为消失点v_i确定的方向向量，为线段l_j的方向向量。

3)平面方向约束：EGMap中的平面均为竖直平面，所以平面π_i的法向量应与竖直消失点的方向O_v平行，即：

4)共面约束：共面约束的代价函数定义为：

其中，和为三维线段l_j的端点的估计值，d_p()表示点到平面的距离。当线段l_j与竖直平面共面时，该代价函数的值为0。

5)可见约束：地图中新增加的几何特征在移动机器人当前的位置必须是可见的，利用已经构建的部分几何地图，判断摄像机与新增的几何特征之间是否存在障碍物的遮挡，即：

6)道路约束：进行地图构建与机器人位置估计时，估计的机器人位置必然位于道路上，否则该位置估计应该被舍弃。因此，定义道路约束的代价函数为：

式(2)中的优化问题可以通过Levenberg-Marquardt算法进行求解。

(6)将三维几何模型与二维几何地图进行匹配，确定二者对应关系，生成城市增强型几何地图；

此步骤中，将上一步骤中获得的三维几何模型与二维几何地图进行匹配，找到三维建筑物模型与二维地图中建筑物轮廓的对应关系。首先，将三维模型向水平平面进行投影；然后，定义代价函数对该投影与二维建筑物轮廓的匹配程度进行度量。由于只考虑三维的竖直平面，所以三维平面的投影为一组线段。对于竖直平面定义Proj(π_i)为其水平投影所产生的线段，和为该线段的两个端点，则该匹配问题转化为求解下面的优化问题：

其中，和是二维建筑物轮廓线段的两个端点，d()表示两点之间的欧式距离。

至此，已经初步生成了移动机器人工作环境的增强型几何地图，下面一步对该估计结果进行进一步优化。

(7)基于带几何约束的局部Bundle Adjustment算法对上述城市增强型几何地图进行优化；

为了进一步提高地图构建的精度，本发明设计了一种带几何约束的局部BundleAdjustment算法来同时对估计的机器人位姿和三维几何地图进行修正和优化。几何约束通过下列代价函数实现：

1)线段重投影误差：定义图像空间下线段的重投影误差为：

其中，P_k是k时刻摄像机的外参数矩阵估计值，l_j是三维线段，是l_j对应的二维图像，和是线段的两个端点，Q(P_k,l_j)表示l_j的重投影线段，Q(P_k,l_j)¹和Q(P_k,l_j)²分别表示该线段的两个端点，函数d_⊥()表示点到直线的欧氏距离，d()表示两点之间的距离，ε(r₁,t₁)是一个代价函数，其中t₁是一阈值，满足当r₁>t₁时该线段在优化过程中会被忽略，否则ε(r₁,t₁)的值等于r₁。

2)平面和共面线段的代价函数：对于三维线段l_j和竖直平面π_i，定义如下代价函数：

其中，l_j∈π_i表示线段l_j位于竖直平面π_i上，d_p()表示点到平面的距离，t₁是一阈值，函数ε()的定义同式(10)。

由于EGMap中所包含的平面均为竖直平面，所以对于平面π_i，其法向量遵循如下约束：n_i＝[n₁,0,n₃]^T。类似地，对于任意竖直直线l_j，其必然要经过三维的竖直消失点，即其中为直线上任意一点，为三维竖直消失点。该约束可以同式(10)和(11)中的约束一同考虑。

3)可见约束代价函数：可见约束的代价函数定义为：

为了避免全局Bundle Adjustment可能出现的由于初始值离优化目标太远而无法收敛的问题，本发明采用局部Bundle Adjustment算法来对初始得到的环境地图进行优化。算法的核心思想为：利用k时刻之前N个关键帧来对m个最近的机器人位姿和k-m+1时刻后新建的环境地图进行重新调整和优化。优化过程的代价函数定义以上各几何约束代价函数的加权和，

其中，w_l和w_π分别为线段重投影误差和共面约束代价的权值，该权值根据经验和实验结果设定。式(13)中的优化问题可以通过Levenberg-Marquardt算法进行求解。

本发明提供的大范围复杂城市环境几何地图自动生成方法的效果可以通过以下实验结果进一步说明。

实验配置和实验数据描述：实验平台为一台移动机器人，上面固定了一台云台摄像机，摄像机型号为Canon VB-H410，摄像机分辨率为960×540像素，视频采集频率为30帧/秒。实验数据通过该车载摄像机在某校园内采集了两段视频，然后利用本发明的方法对视频数据进行处理，重建增强型几何地图。图5中，(a)为实验平台，(b)和(d)为两个实验场景的卫星地图，其中的加粗虚线表示为移动机器人设定的运动轨迹，(c)和(e)分别为场景(b)和场景(d)中两幅示意图像帧。

首先，对三维几何模型与二维几何地图匹配的结果进行评估，结果显示，两组实验中一共重建了36个竖直平面，全部竖直平面均与二维地图中的建筑物轮廓信息正确匹配。说明本发明方法具有较好的鲁棒性和准确性。

然后，为了评估本发明提供的大范围复杂城市环境几何地图自动生成方法的效果，定义如下地图生成精度评价指标：定义ε_d和ε_θ为竖直平面的距离误差均值和方向误差均值，竖直平面的真实值通过三个位于该竖直平面内的不共线的点计算得到，位于竖直平面内的点的三维坐标通过激光测距仪测量得到，激光测距仪的型号为BOSCH GLR225，测量距离为0-70米，误差为±1.5毫米。ε_d定义如下：

其中，N_p为提取的竖直平面总数，d_i为竖直平面π_i与摄像机中心之间距离的真实值，为竖直平面π_i距离的估计值。

类似地，定义ε_θ为：

其中，n_i为竖直平面π_i法向量的真实值，为竖直平面π_i法向量的估计值。

为了评估三维线段的重建精度，定义三维线段在图像空间中的重投影误差ε_L如下：

其中，为三维线段估计在图像空间中的投影，和分别为该三维线段在图像空间中投影的观测值的两个端点，N_L为所有重建的三维线段的总数，函数d_⊥()表示点到直线的几何距离。

表1给出了在上述两个实验场景中几何地图生成的结果。从表中可以看出，所有实验结果中，竖直平面的距离误差不大于0.82米，方向误差不大于6.5度，线段的重投影误差不大于1.4个像素。说明本发明提供的大范围复杂城市环境几何地图自动生成方法具有较高的准确性。

表1两组实验中地图生成结果

虽然利用本发明方法所生成的几何地图所需存储空间的大小依赖于场景的大小以及场景中建筑物和道路的复杂程度，然而在本实验的两个实验场景中(机器人运动轨迹均为500米左右)，利用本发明提供的大范围复杂城市环境几何地图自动生成方法所生成的几何地图所需存储空间均不超过10MB，在一定程度上说明本发明方法所需的存储空间较小。

Claims (2)

1.一种大范围复杂城市环境几何地图生成方法，其包括按顺序进行的下列步骤：

(1)提取二维道路及二维建筑物轮廓信息，生成二维几何地图；

(2)由二维几何地图引导车载单目云台摄像机进行视频采集，由此获得视频图像；

(3)从步骤(2)中得到的视频图像中提取出关键帧；

(4)提取与匹配相邻关键帧之间的线段特征，由此得到线段对应；

(5)基于匹配的线段特征的移动机器人同时定位与环境三维几何模型构建；

(6)将三维几何模型与二维几何地图进行匹配，确定二者对应关系，生成城市增强型几何地图；

(7)基于带几何约束的Bundle Adjustment算法对上述城市增强型几何地图进行优化；

其特征在于：在步骤(5)中，所述的基于匹配的线段特征的移动机器人同时定位与环境三维几何模型构建的方法是：共包括初始估计和优化估计两个阶段；在初始估计阶段，利用获得的相邻关键帧间的匹配线段，基于RANSAC算法估计出基本矩阵F，然后利用摄像机内参数矩阵和基本矩阵F估计出本质矩阵E，之后通过分解本质矩阵E估计出移动机器人的位姿变化，最后利用三角法重构得到三维空间中的线段，并基于共面线段估计出相应的竖直平面；在优化估计阶段，利用各种特征之间的几何约束，通过构造并求解优化函数完成对初始估计结果的优化，以降低估计误差随机器人运动范围的扩大而不断累积。

2.根据权利要求1所述的大范围复杂城市环境几何地图生成方法，其特征在于：在步骤(7)中，所述的基于带几何约束的Bundle Adjustment算法对上述城市增强型几何地图进行优化的方法是：首先，基于城市增强型几何地图中各种特征之间的几何约束，分别构造线段重投影误差、共面约束代价函数、可见约束代价函数；然后，基于局部Bundle Adjustment算法对几何地图进行优化：利用k时刻之前N个关键帧来对m个最近的机器人位姿和k-m+1时刻后新建的环境地图进行重新调整和优化；优化过程的代价函数定义为构造的几何约束代价函数的加权和，利用Levenberg-Marquardt算法对该优化问题进行求解。