基于混合特征的移动机器人级联式地图创建方法

1.一种基于混合特征的移动机器人级联式地图创建方法，其特征在于包括以下步骤：
步骤一，获取激光数据点；
步骤二，提取几何特征并进行特征匹配；
步骤三，提取SURF特征点并进行特征匹配；
步骤四，建立级联式地图，方法如下：
(1)创建全局拓扑地图
在已建立的局部信息基础上，建立全局拓扑地图，用集合T代表已生成的拓扑节点，当前拓扑节点Ti描述为：
Ti＝{Q,G,V,p}
式中，Q代表当前节点存在的可能点，G为Ti处所包含的局部几何特征，V是Ti处所提取的SURF特征向量，p表示机器人当前位姿；
拓扑地图建立过程中实时更新拓扑节点，利用Dijkstra方法计算两节点间的最短路径，即：
dk(j)＝min{dk-1(j),dk-1(i)+d(i,j)}
式中，dk(j)表示当前位置与第j个节点的距离，d(i,j)是第i个节点与第j个节点的距离；
(2)类‘U’型和类‘L’型环境处理
在环境探索过程中，类‘U’型环境和类‘L’型环境的判断准则为：
a.当前激光扫描区域内存在U型、L型线段；
b.可能目标点在封闭区域内；
c.当前位置与可能点距离小于一定阈值；
类‘U’型环境和类‘L’型环境可能点是规则区域中的特殊情况，具有终止性，可作为拓扑节点存放，拒绝对封闭区域的探索，以此提高环境遍历的效率；
步骤五，分割房间；
步骤二所述的几何特征提取与匹配，是将环境信息分为规则区域和非规则区域，在这两个区域分别进行几何特征提取，然后进行几何特征匹配；具体方法如下：
(1)规则区域提取几何特征
针对规则区域，首先通过阈值比较法检测断点，利用断点将所有采集的激光数据点分为若干个点集，并将断点作为这些点集的起始点和终结点；然后进行拐点检测，取出断点检测所得的一个点集Pi，将其二等分得到点集Li1和Li2；分别对点集Li1和Li2内的数据点进行线段拟合，若所得的两线段的角度差Φ大于阈值Φth，则该二等分点即为拐点；直至等分的所有点集Pi可用一条线段表示，得到点集(Li1,Li2...,Lim)；最后采用最小二乘线段拟合方法对数据点进行线性拟合并计算出线段的特征参数，将线段中点作为几何特征点存入拓扑节点中；
(2)非规则区域提取几何特征
采用改进的基于网格的共享近邻方法实现对不规则区域的聚类与划分，其具体步骤如下：
a.将N个激光数据点所在区域A划分为T个网格单元；
b.计算密度阈值minY，公式如下：
式中，GN为非空网格数，MaxG为单元网格中激光数据点数最大值；
c.判断高、低密度单元；
若一个网格单元中激光数据点个数大于密度阈值minY，则认为该网格单元为高密度单元；反之，为低密度单元；
d.对于高密度单元，利用求该单元内激光数据点平均值的方法计算其中心点P，作为聚类椭圆的中心，公式如下：
式中，n为该单元中属于高密度单元的激光数据点的个数；
该单元内所有激光数据点与中心点P之间的最大距离记作MaxL，以此作为聚类椭圆的长半径；同时，长半径垂直方向上的激光数据点与中心点P之间的最大距离记作MaxD，以此作为聚类椭圆的短半径；
e.对于低密度单元，将单元内的激光数据作为噪声进行处理；
f.将椭圆中心作为几何特征点，存于拓扑节点中作为该节点的辨识依据；
(3)几何特征匹配
ICP方法是基于最小二乘法的最优匹配方法；为找到目标点集与参考点集之间的旋转和平移变换，使得两组匹配数据之间满足某种度量标准下的最优匹配，重复进行参考点集和目标点集间的转换过程，直到最小误差满足一定的标准要求，即：
式中，Edist为误差和，R为旋转矩阵，T是平移向量，α是旋转角；P(P1,P2...,Pm)为目标点集，Q(Q1,Q2...,Qm)为参考点集；通过最小化误差和Edist求两个点集之间的相对坐标变换x＝(α,T)；
使用ICP对机器人位姿初始校正后，通过欧氏距离利用每个拓扑节点所含的几何特征点计算校正后的环境中拓扑节点之间的匹配度；首先选择待匹配的拓扑节点，计算生成拓扑节点与待匹配节点间的特征点距离，若特征点Pi与特征点Qi小于给定阈值且距离最近，则计算两者之间匹配度δi，得到几何特征匹配度ε：
n为待匹配拓扑节点中几何特征点的个数；
步骤三所述的提取SURF特征点并进行特征匹配的方法如下：
(1)通过对图像子区域的快速求和计算积分图像
对于图像中某点X＝(x,y)的积分I∑(X)为：
式中，I(i,j)为给定图像的灰度值；
(2)求取近似Hessian矩阵Happrox
对于图像I中某点X＝(x,y)，在X点的s尺度上的Hessian矩阵H(X,s)定义为：
式中，Lxx(X,s),Lxy(X,s),Lyy(X,s)表示高斯二阶偏导数在X处与图像I的卷积；使用方框滤波近似代替Hessian矩阵中的二阶高斯滤波；框状滤波模板同图像卷积后的值分别为Dxx、Dyy、Dxy，进一步得到近似Hessian矩阵Happrox，其行列式为：
2
det(Happrox)＝DxxDyy-(wDxy)
式中，w是权重系数；
(3)对特征点进行定位
用不同尺寸的框状滤波器对原始图像进行处理得到尺度图像金字塔，根据Happrox求出尺度图像在(X,s)处的极值，在3维(X,s)尺度空间中，对每个3×3×3的局部区域进行非极大值抑制，即保留极大值，其它值置为0；将响应值大于26个邻域值的点选为特征点；利用二次拟合函数对特征点精确定位，拟合函数D(X)为：
至此，得到了特征点的位置、尺度信息(X,s)；
(4)确定特征点的方向特征
用Haar小波滤波器对圆形邻域进行处理，得到该邻域内每个点所对应的x、y方向的响应；选取以特征点为中心的高斯函数，σ取2s，s为该特征点对应的尺度，对这些响应进行加权，搜索长度最大的矢量，其方向即为该特征点所对应的方向；
(5)构建特征描述向量
以特征点为中心确定一个正方形的邻域，边长取20s，把特征点方向设定为该邻域的y轴方向；把正方形区域分成4×4个子区域，在每一个子区域中用Haar小波滤波器进行处理；dx表示水平方向的Haar小波响应，dy表示竖直方向的Haar小波响应；对于所有的dx、dy用以特征点为中心的高斯函数加权，该高斯函数的σ为3.3s；在每个子区域分别对dx、dy、|dx|、|dy|求和，得到一个4维向量V(∑dx,∑dy,∑|dy|,∑|dy|)；把4×4个子区域的向量连接起来就得到了一个64维的向量，得到描述特征点的特征向量；
(6)进行特征匹配
a.利用随机取出的大于6的n组角点匹配对组成一个样本作为能量函数的输入信息，计算单应性矩阵M；
b.判断此时对应的能量函数的值，若该值小于阈值ζ，则继续；否则，返回a；
c.对通过SURF方法获取的所有角点匹配对进行内点判断，即计算对应点间的距离d，若d小于阈值d0，则设置为内点；若内点数量小于阈值N则返回a；
d.输入所有内点并获取当前能量函数值，若能量函数值小于域值δ，则匹配成功；否则，返回a；
e.若计算获得的M无法满足在b或d中的阈值条件时，则匹配失败；
步骤五所述的房间分割的方法如下：
(1)进行综合匹配计算，公式如下：
ηi＝εi+kξiδ(εi)
式中，ηi为综合匹配度，k为调节系数，εi为基于几何特征的的认知匹配率，ξi为基于SURF特征的的认知匹配率，δ(εi)为认知参数，当几何特征的认知匹配率小于阈值ε′时设为0；
对于每个节点所包含的几何特征点进行匹配；其中两节点i与j对应综合特征匹配度超过阈值λ，则认为两节点相似，即matched；为消除SURF误匹配对权值的影响，引入节点间距离信息来提高聚类结果的准确性；
(2)分割房间
定义无向加权图中相似矩阵为：
式中，μ(Si，Sj)表示两节点间最短路径距离，μ0为高斯分布的随机变量的均值，σ为高斯分布的随机变量的方差，dij代表高斯核修正后的距离值，用来控制节点间位置值变换的敏感程度；
房间分割的方法如下：
a.根据相似矩阵求取无向加权图的对称相似矩阵W，将相似矩阵每行元素相加即得到顶点的度，基于此，获取度矩阵D，其中Dii＝di；基于D和W形成normalized拉普拉斯矩阵：
b.计算矩阵L的特征值与特征向量，构成特征向量空间；
c.利用k-means聚类方法对特征向量空间中的特征向量进行聚类，特征向量分类结果映射到其相应的顶点，将顶点进行组划分，最终获得房间分割图。

基于混合特征的移动机器人级联式地图创建方法\n技术领域\n[0001] 本发明属于智能移动机器人的领域，涉及未知环境下移动机器人利用混合特征实现级联式地图构建方法。\n背景技术\n[0002] 近年来，自主移动机器人已成为当今机器人技术的一个重要领域，被广泛应用于工业、医疗及军事等领域。在机器人技术中，机器人地图构建是目前的一个热点，学者们已经进行了很多研究，并提出了不同的地图表示方法。可以将机器人地图分为以下两大类：几何地图和拓扑地图，同时几何地图又可分为栅格地图和特征地图，不同的地图创建方法有各自的优点和局限性。\n[0003] 栅格地图，是一种基于栅格的地图表示方法，将整个环境分为若干大小相同的栅格，栅格中的每一个单元代表环境的一部分，对于每个栅格中是否存在障碍物，每一个栅格都给设定了一个概率值，用来表示改栅格被障碍物占据的可能性大小。栅格图不同于特征法的一个地方，它不需要对应用的特征信息环境进行精确的描述，并且栅格表示地图属于近似描述，对某个栅格的感知信息可直接与探测环境中的某个区域对应，所以栅格地图易于创建和维护，并且使用声纳这样的廉价传感器也可以进行地图创建，借助栅格地图，可以方便地进行自主定位和路径。但当栅格数量增大时，在大型环境中或者地图建模中的分辨率提高时，地图创建所需的内存和CPU资源迅速增长，使得计算量相应提高。\n[0004] 特征地图，是一种基于特征信息的地图表示方法，主要是依赖机器人对所探测环境的信息收集，从中提取抽象的具有几何特征的数据。特征图建模是种很常用的一种方法，在某些特定的室内结构化环境中，最常见的特征就是线段、角、边，将环境定义为面、角、边的集合或者墙、走廊、门、房间等等，这些具有几何特征信息一般使用了颜色、长度、宽度、坐标等一系列的参数来表示，使用这些几何信息描述环境能够使得地图的描述更为紧凑，且便于位置估计和目标识别。特征方法建模定位准确，环境模型易于被描绘与表示，地图的参数化设置也适用于路径规划与运动控制，但特征法需要特征提取等预处理过程，且需要一定数量的感知数据才能得到结果，对传感器噪音也比较敏感，只适于高度结构化环境。\n[0005] 拓扑地图，是一种比较紧凑的地图表示方法，一般应用于室内环境，通常是根据环境结构定义的，所以环境的复杂度决定了拓扑图的分辨率，图中的节点与节点间的连线有着严格的对应准则，拓扑图可组织为层次结构，这种表示方法可以实现快速的路径规划，并且为多线程的人机交互指令的下达提供了一个更为理想的接口。拓扑图把环境建模成一张拓扑意义中的图，忽略了具体的几何特征数据，不必精确描述不同节点间的地理位置关系，通过抽象的理论来直观地描述环境，所以拓扑地图对机器人位姿信息的准确度要求并不高，对于机器人的位姿误差有了更好的鲁棒性。当机器人离开一个节点时，机器人只要知道它从哪条连线行走就足够了，当遇到拐角处，一般只需要辨别4个方位，而这些都可以通过机器人自身的里程计与罗盘来实现。在拓扑图中进行定位，机器人必须准确地分辨节点，因此节点要求有明显可区分和辨别的标识或者特征，并能被传感器识别，如果探测环境中存在两个或者两个以上相似地方时，并且机器人从不同路径进行探测时，使用拓扑表示方法就很难分辨是否为同一节点。即拓扑图表示简单，易于扩展，但在精确识别具体位置上有着很大的不足。\n[0006] 不同的地图创建方法有各自的优点和局限性，因此，很多学者结合几种地图的优点引入混合地图的创建。但是单纯的地图构建都侧重于空间几何结构的表示，适用于导航，未考虑环境中的区域性特征，且未对已探索信息进行相应的关联，没有考虑机器人服务工作的区域性特征和局部服务空间的复杂性，因此不能对服务型操作任务提供更丰富的信息。一些学者提出基于3D点云方法，但该方法对视觉信息处理要求高，且只适用于特定环境。室内环境采用分层方式建模是近几年地图构建的新模式，有学者提出面向大规模室内环境提出用分层的几何—拓扑3维地图管理广域环境特征，定义了不同层次的3维局部环境特征及全局拓扑属性。还有学者针对结构化的简单环境，采用不同的地图表示方法提出了两层地图模型．但这些分层地图都仅适用于导航，对于机器人的服务操作任务不能提供丰富的房间信息，还需进一步丰富环境信息。\n发明内容\n[0007] 为解决单独一种地图创建的缺陷，同时为提供丰富的房间信息，本发明结合几种机器人地图的优点，创建一种融合局部信息层、全局拓扑地图层、房间分割层的级联式地图。该级联式地图不仅包含环境探索过程中生成的拓扑地图，并且根据已探索的区域性特征为服务型任务提供了房间分割信息。\n[0008] 本发明采用的技术方案如下：\n[0009] 利用激光传感器获取激光数据点，得到环境信息；探索过程中，首先获得移动机器人可通行空间及可能目标点；对环境信息进行提取，针对准结构化环境，将几何特征划分为规则区域和非规则区域；同时，在非规则区域采用改进的基于网格的共享近邻方法(grid based shared nearest neighbor clustering algorithm,GNN方法)进行非规则聚类，将得到的几何特征存入拓扑节点中，改善拓扑节点辨识度，以提高机器人环境认知能力；提取SURF（Speeded-Up Robust Features）特征关联不同拓扑节点，并采用RANSAC方法对匹配特征点对进行优化，以降低方法的误匹配率；建立全局拓扑地图，在此基础上为提供丰富的信息，利用拓扑节点所包含的几何特征与SURF综合匹配值构造无向加权图，借助谱聚类理论与特征匹配信息对全局拓扑图进行区域划分，进而实现房间分割层构建。\n[0010] 本发明所述方法包括以下步骤：\n[0011] 步骤一，获取激光数据点。\n[0012] 激光测距是激光技术应用最早的一个领域，激光测距具有探测距离远，测量精度高等特点。激光测距雷达通过获取距离数据，为机器人的导航提供了便捷有效的环境描述。\n激光测距仪测量精度高，而且散射角很小，光波的反射性能决定了数据的可靠性，不需要考虑多次反射和错误反射的问题，读数可以直接作为真实距离使用，结构简单，测距速度快、系统稳定可靠。\n[0013] 本发明选择了日本北阳公司生产的URG-04LX型激光测距仪，作为外部传感器获知环境信息。URG-04LX是一款高性能、价格相对较低的激光扫描测距传感器，专门为机器人应用设计。其内部主要由微控制芯片、激光发射器、激光接收器、高速旋转镜片和反射镜构成。通过高速旋转镜片的转动可以实现240度范围内的扫描，有效范围内所采集到的有效数据点的个数为682个。经过微控制芯片处理得到的距离信息通过串口(RS232/USB)传送给上位机。其高精度、高分辨率、宽视场的特点，非常适用于机器人在未知环境中自主移动。\n[0014] 步骤二，提取几何特征并进行特征匹配。\n[0015] 为更好完成自主环境认知和获取足够的信息，对几何信息进行提取，同时将环境信息分为规则区域和非规则区域，存入拓扑节点中增加拓扑节点认知度，从而在机器人探索未知环境时对不同的拓扑节点进行识别，提高地图创建效率。\n[0016] (1)规则区域提取几何特征\n[0017] 本发明采用“断点检测—拐点检测—线段拟合”的三步法进行数据处理，将数据信息用几何线段描绘出来，并提取线段中点作为特征点，存入相应的拓扑节点中作为拓扑节点的辨识依据。\n[0018] (2)非规则区域提取几何特征\n[0019] 本发明使用椭圆对其进行描述，采用改进的基于网格的共享近邻方法实现对不规则区域的聚类与划分，并将聚类后的椭圆中心点作为特征点存于相应节点中作为拓扑节点的辨识依据。\n[0020] (3)进行几何特征匹配\n[0021] 为有效提高拓扑节点的位置精度，本发明将迭代最近点（Iterative Closest Point）ICP方法引入到机器人探索任务中，防止因位置误差不断累积而导致地图数据关联失败。通过迭代不断更新机器人的位置，对环境进行初始校正后，利用欧氏距离对校正后环境中不同拓扑节点所含特征点进行匹配，计算不同节点间的匹配度。\n[0022] 步骤三，提取SURF特征点并进行匹配。\n[0023] 通过分析图像信息，决定图像中的每个点是否属于一个图像特征。特征提取的结果是把图像上的点划分为不同的子集，这些子集往往属于孤立的点、连续的曲线或者连续的区域。加速鲁棒特征SURF特征点是目前计算图像特征最流行的方法，该方法提取的特征具有尺度不变、旋转不变的性能，同时对光照变化和仿射、透视变换具有不变性。\n[0024] 在机器人环境探索过程中，为描述更丰富的环境信息，本发明将SURF信息融入到拓扑节点中。提取SURF特征点，包括特征点检测和特征点描述两个部分，并采用基于欧氏距离的最邻近法进行特征点匹配，利用K-D树的数据结构进行搜索，根据最近两个特征点的距离比确定是否接受这一匹配对。\n[0025] 步骤四，建立级联式地图。\n[0026] 为创建融合局部信息层、全局拓扑地图层、房间分割层的级联式地图，本发明首先建立融合SURF特征及几何特征的局部信息层作为基层地图；在此基础上，建立全局拓扑地图层，并在建立过程中实时更新拓扑节点，并利用Dijkstra方法计算两节点间的最短路径，同时对类U型、类L型环境进行特殊处理，提高探索效率；最后建立房间分割层，以提供丰富的房间信息。\n[0027] 步骤五，分割房间。\n[0028] 在生成的拓扑地图基础上，根据拓扑地图生成对应的无向加权图，结合基于规范割集准则(Min-Ncut)的谱聚类方法对全局拓扑图进行房间分割。并利用节点间路径信息与SURF、几何综合认知匹配值构造聚类相似矩阵，以提高聚类结果的准确性。\n[0029] 本发明的有益效果是：本发明可创建一种融合局部信息层、全局拓扑地图层、房间分割层的级联式地图，解决了栅格地图、特征地图和拓扑地图等单独一种地图创建的缺陷，可提供丰富的房间信息。\n附图说明\n[0030] 图1为基于混合特征的移动机器人级联式地图创建方法流程图；\n[0031] 图2为几何特征提取流程图；\n[0032] 图3为SURF特征点提取与匹配流程图；\n[0033] 图4为本发明应用实例创建的地图：图4(a)为机器人从起点自主运动到终点的过程中截取的四个时刻生成的拓扑地图，图4(b)为相应四个时刻的当前拓扑节点所包含的几何特征信息；\n[0034] 图5为本发明应用实例房间分割的示意图。\n具体实施方式\n[0035] 下面结合附图对本发明作进一步的详细说明。\n[0036] 本发明所述方法的主流程图如附图1所示，包括以下几个步骤：\n[0037] 步骤一，获取激光数据点，具体方法如下：\n[0038] 激光传感器测量范围为20～4000mm；角度分辨率：0.36°；扫描时间：100ms。其特点为精度高、分辨率高、测量范围广。\n[0039] 激光传感器采集数据原理：\n[0040] 第i个数据点对应的角度θi为：\n[0041] \n[0042] 将极坐标转换直角坐标，得到数据点坐标（Xi,Yi）：\n[0043] Xi＝ρi×cosθi,Yi＝ρi×sinθi\n[0044] 式中，ρi为激光传感器测量到的第i个数据点与机器人间距离。\n[0045] 步骤二，提取几何特征并进行特征匹配。\n[0046] 本发明对几何信息进行提取的具体流程图如图2所示。将环境信息分为规则区域和非规则区域，存入拓扑节点中增加拓扑节点认知度。在规则区域中，采用“断点检测—拐点检测—线段拟合”的三步法进行数据处理，在此基础上将数据信息用几何线段描绘出来，并提取线段中点作为环境特征点，作为拓扑节点的辨识依据；对于非规则区域，本发明使用椭圆对其进行描述，采用改进的基于网格的共享近邻方法实现对不规则区域的聚类与划分，并将聚类后的椭圆中心点作为环境特征点存于级联式地图中作为拓扑节点的辨识依据，对拓扑节点中所含的几何特征进行匹配得到匹配度。为降低误差对后续基于几何特征的环境认知产生的影响，引入ICP(Iterative closest Point)方法对机器人位姿进行矫正。\n[0047] （1）规则区域提取几何特征\n[0048] 针对规则区域，首先通过阈值比较法检测断点，利用断点将所有采集的激光数据点分为若干个点集，并将断点作为这些点集的起始点和终结点。然后进行拐点检测，取出断点检测所得的一个点集Pi，将其二等分得到点集Li1和Li2。分别对点集Li1和Li2内的数据点进行线段拟合，若所得的两线段的角度差Φ大于阈值Φth，则该二等分点即为拐点。\n直至等分的所有点集Pi可用一条线段表示。至此，完成了点集的拐点检测，可得到点集（Li1,Li2...,Lim)。最后采用最小二乘线段拟合方法对数据点进行线性拟合并计算出线段的特征参数，将线段中点作为几何特征点存入拓扑节点中。\n[0049] （2）非规则区域提取几何特征\n[0050] 采用改进的基于网格的共享近邻方法实现对不规则区域的聚类与划分，其具体步骤如下：\n[0051] a.将N个激光数据点所在区域A划分为T个网格单元；\n[0052] b.计算密度阈值minY，方法如下：\n[0053] \n[0054] 式中，GN为非空网格数，MaxG为单元网格中激光数据点数最大值。\n[0055] c.判断高、低密度单元\n[0056] 若一个网格单元中激光数据点个数大于密度阈值minY，则认为该网格单元为高密度单元；反之，为低密度单元。\n[0057] d.对于高密度单元，利用求该单元内激光数据点平均值的方法计算其中心点P，作为聚类椭圆的中心，方法如下：\n[0058] \n[0059] 式中，n为该单元中属于高密度单元的激光数据点的个数。\n[0060] 该单元内所有激光数据点与中心点P之间的最大距离记作MaxL，以此作为聚类椭圆的长半径；同时，长半径垂直方向上的激光数据点与中心点P之间的最大距离记作MaxD，以此作为聚类椭圆的短半径。\n[0061] e.对于低密度单元，将单元内的激光数据作为噪声进行处理。\n[0062] f.将椭圆中心作为几何特征点，存于拓扑节点中作为该节点的辨识依据。\n[0063] （3）几何特征匹配\n[0064] ICP方法是基于最小二乘法的最优匹配方法。为找到目标点集与参考点集之间的旋转和平移变换，使得两组匹配数据之间满足某种度量标准下的最优匹配，重复进行参考点集和目标点集间的转换过程，直到最小误差满足一定的标准要求，即：\n[0065] \n[0066] \n[0067] 式中，Edist为误差和，R为旋转矩阵，T是平移向量，α是旋转角。P（P1,P2...,Pm)为目标点集，Q（Q1,Q2...,Qm)为参考点集。通过最小化误差和Edist求两个点集之间的相对坐标变换x=(α,T)。\n[0068] 使用ICP对机器人位姿初始校正后，本发明通过欧氏距离利用每个拓扑节点所含的几何特征点（即几何特征提取得到的线段中点及椭圆中心），计算校正后的环境中拓扑节点之间的匹配度。首先选择待匹配的拓扑节点，计算生成拓扑节点与待匹配节点间的特征点距离，若特征点Pi与特征点Qi小于给定阈值且距离最近(规则区域特征点与规则区域特征匹配，非规则区域特征点与非规则区特征匹配)，则计算两者之间匹配度δi，得到几何特征匹配度ε：\n[0069] \n[0070] n为待匹配拓扑节点中几何特征点的个数。\n[0071] 步骤三，提取SURF特征点并进行特征匹配，流程图如图3所示，具体方法如下：\n[0072] （1）通过对图像子区域的快速求和来计算积分图像，即计算给定灰度图像所有像素的累积和。\n[0073] 对于图像中某点X＝(x,y)的积分I(X)为：\n[0074] \n[0075] 式中，I(i,j)为给定图像的灰度值。\n[0076] 在积分图像中利用方框滤波近似代替二阶高斯滤波，大大提高了运算效率。\n[0077] （2）求取近似Hessian矩阵Happrox。\n[0078] 对于图像I中某点X＝(x,y)，在X点的s尺度上的Hessian矩阵H(X,s)定义为：\n[0079] \n[0080] 式中，Lxx(X,s),Lxy(X,s),Lyy(X,s)表示高斯二阶偏导数在X处与图像I的卷积。\n使用方框滤波近似代替Hessian矩阵中的二阶高斯滤波。框状滤波模板同图像卷积后的值分别为Dxx、Dyy、Dxy，进一步得到近似Hessian矩阵Happrox，其行列式为：\n[0081] det(Happrox)＝DxxDyy-(wDxy)2\n[0082] 式中，w是权重系数，在本发明的实施中取值为0.9。\n[0083] （3）对特征点进行定位。\n[0084] 用不同尺寸的框状滤波器对原始图像进行处理得到尺度图像金字塔，根据Happrox求出尺度图像在(X,s)处的极值，在3维(X,s)尺度空间中，对每个3×3×3的局部区域进行非极大值抑制（保留极大值，其他值置为0）。将响应值大于26个邻域值的点选为特征点。\n利用二次拟合函数对特征点精确定位，拟合函数D(X)为：\n[0085] \n[0086] 至此，得到了特征点的位置、尺度信息(X,s)。\n[0087] （4）确定特征点的方向特征。\n[0088] 用Haar小波滤波器对圆形邻域进行处理，得到该邻域内每个点所对应的x、y方向的响应。选取以特征点为中心的高斯函数（σ取2s，s为该特征点对应的尺度），对这些响应进行加权，搜索长度最大的矢量，其方向即为该特征点所对应的方向。\n[0089] （5）构建特征描述向量。\n[0090] 以特征点为中心确定一个正方形的邻域，边长取20s，把特征点方向设定为该邻域的y轴方向。把正方形区域分成4×4个子区域，在每一个子区域中用Haar小波滤波器进行处理。dx表示水平方向的Haar小波响应，dy表示竖直方向的Haar小波响应。对于所有的dx、dy用以特征点为中心的高斯函数加权，该高斯函数的σ为3.3s。在每个子区域分别对dx、dy、|dx|、|dy|求和，得到一个4维向量V(Σdx,Σdy,Σ|dy|,Σ|dy|)。把4×4个子区域的向量连接起来就得到了一个64维的向量，至此，得到了描述特征点的特征向量。\n[0091] （6）进行特征匹配。\n[0092] SURF特征提取后，采用基于K-D树的最邻近点搜索方法进行特征匹配，如果相似度满足阈值要求，则认为特征匹配成功。在实际匹配过程中，利用上述最临近匹配方法得到的特征点间对应关系可能存在具有一定程度的误匹配信息，因此采用RANSAC方法以提高SURF匹配方法的鲁棒性和准确性。基本计算过程如下：\n[0093] a.利用随机取出的n(n>6)组角点匹配对组成一个样本作为能量函数的输入信息，计算单应性矩阵M；\n[0094] b.判断此时对应的能量函数的值，若该值小于阈值ζ，则继续；否则，返回a；\n[0095] c.对通过SURF方法获取的所有角点匹配对进行内点判断，即计算对应点间的距离d，若d小于阈值d0，则设置为内点。若内点数量小于阈值N则返回a；\n[0096] d.输入所有内点并获取当前能量函数值，若能量函数值小于域值δ，则匹配成功；否则，返回a；\n[0097] e.若计算获得的M无法满足在b或d中的阈值条件时，则匹配失败。\n[0098] 步骤四，建立级联式地图。\n[0099] 本发明建立融合局部信息层、全局拓扑地图层及房间分割层的级联式地图。如图\n4所示，作为基层的局部信息层融合了几何特征及SURF特征，提供充分的环境信息；建立全局拓扑地图过程中，将局部信息存入拓扑节点中，增加节点辨识度；最后建立房间分割层，丰富房间信息。\n[0100] （1）创建全局拓扑地图\n[0101] 在已建立的局部信息基础上，建立全局拓扑地图，用集合T代表已生成的拓扑节点，则当前拓扑节点Ti可描述为：\n[0102] Ti＝{Q,G,V,p}\n[0103] 式中，Q代表当前节点存在的可能点，G为Ti处所包含的局部几何特征，V是Ti处所提取的SURF特征向量，p表示机器人当前位姿。\n[0104] 拓扑地图建立过程中实时更新拓扑节点，利用Dijkstra方法计算两节点间的最短路径，即：\n[0105] dk(j)＝min{dk-1(j),dk-1(i)+d(i,j)}\n[0106] 式中，dk(j)表示当前位置与第j个节点的距离，d(i,j)是第i个节点与第j个节点的距离。\n[0107] （2）类‘U’型和类‘L’型环境处理。\n[0108] 在环境探索过程中，类‘U’型环境和类‘L’型环境的判断准则为：\n[0109] a.当前激光扫描区域内存在U型、L型线段；\n[0110] b.可能目标点在该封闭区域内；\n[0111] c.当前位置与可能点距离小于一定阈值。\n[0112] 类‘U’型环境和类‘L’型环境可能点是规则区域中的特殊情况，其具有终止性，可作为拓扑节点存放，拒绝对封闭区域的探索，以此提高环境遍历的效率。\n[0113] 步骤五，分割房间。\n[0114] 本发明将拓扑节点位置对应于无向加权图中的顶点，利用SURF特征以及几何特征综合匹配信息及节点间的最短路径作为无向加权图中顶点间连线权值。\n[0115] 谱图理论中，将无向加权图G划分为A、B两个子图，A∪B=V，A∩B=φ。Shi和Malik根据谱图理论建立2-way划分的规范割集准则目标函数：\n[0116] \n[0117] \n[0118] \n[0119] 式中，Ncut(A,B)为规范割集准则目标函数，cut(A,B)为将无向加权图G划分为A、B两个子图的代价函数，assoc(A，V)为所有连通点之间的连接权值之和，w(u,t)代表两个子图A、B中任意两点u，v间的连通性。最小化Ncut函数称为规范割集准则，该函数不但能衡量样本间相似程度，同时也可以区分样本间差异程度。\n[0120] 具体方法如下：\n[0121] （1）进行综合匹配计算，公式如下：\n[0122] ηi＝εi+kξiδ(εi)\n[0123] \n[0124] 式中，ηi为综合匹配度，k为调节系数，εi为基于几何特征的的认知匹配率，ξi为基于SURF特征的的认知匹配率，δ(εi)为认知参数，当几何特征的认知匹配率小于阈值ε′时设为0。\n[0125] 对于每个节点所包含的几何特征点进行匹配。其中两节点i与j对应综合特征匹配度超过阈值λ，则认为两节点相似，即matched。为消除SURF误匹配对权值的影响，引入节点间距离信息来提高聚类结果的准确性。\n[0126] （2）分割房间\n[0127] 定义无向加权图中相似矩阵如下：\n[0128] \n[0129] 式中，μ(Si，Sj)表示两节点间最短路径距离，μ0为高斯分布的随机变量的均值，σ为高斯分布的随机变量的方差，dij代表高斯核修正后的距离值，用来控制节点间位置值变换的敏感程度。\n[0130] 将无向加权图中的节点按空间交叠的情况分成集合，组内节点间相似度最高，组间的相似度弱，从而实现对全局地图的区域分割。具体方法如下：\n[0131] a.根据相似矩阵求取无向加权图的对称相似矩阵W，将相似矩阵每行元素相加即得到顶点的度，基于此，获取度矩阵D，其中Dii=di。基于D和W形成normalized拉普拉斯矩阵：\n[0132] \n[0133] b.计算矩阵L的特征值与特征向量，构成特征向量空间。\n[0134] c.利用k-means聚类方法对特征向量空间中的特征向量进行聚类，特征向量分类结果映射到其相应的顶点，将顶点进行组划分，最终获得房间分割图。\n[0135] 下面给出应用本发明所述方法在实验环境下进行级联式地图创建的一个实验实例。\n[0136] 采用Pioneer3-DX机器人在实验环境下利用激光传感器采集环境信息，机器人从起点自主运动到终点的过程中截取的四个时刻生成的拓扑地图如图4(a)所示；图4(b)为相应四个时刻的当前拓扑节点所包含的几何特征信息，线段为提取的规则区域中的几何线段特征，椭圆即为非规则区域聚类提取出的几何特征。实现了将几何特征融合到拓扑节点中的目的，增加了拓扑节点所包含的几何信息，提高了节点的辨识度。在此基础上，对节点进行匹配进而得到房间分割结果，示意图如图5所示，x轴代表长度，y轴代表宽度，不同节点间连线代表两节点存在关联，通过房间分割更好地得到了房间信息。\n[0137] 实验结果表明，本发明所述方法可用于未知环境下级联式地图创建，能够克服传统拓扑地图中拓扑节点所含几何环境信息少不能精确定位的缺点，提高环境认知能力；同时考虑机器人服务工作的区域性特征和局部服务空间的复杂性，对已探索信息进行相应的关联，提供丰富的房间信息。\n[0138] 以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。