CN105841687B - 室内定位方法和系统 - Google Patents

Abstract

本发明涉及室内导航领域，公开了一种室内定位方法和系统。本发明中，通过在室内的固定位置部署视觉路标(人工路标或者自然路标)，在定位过程中，移动机器人拍摄周边环境的图像，对拍摄的图像进行预处理和特征提取，将提取特征后的特征图像和路标特征库中的路标进行匹配，得到图像中可用于定位的路标；针对特征路标采用视觉测距方案对移动机器人进行实时定位。本发明的实施方式由于采用视觉路标(被动式路标)，因此路标本身不发射无线信号，路标本身成本较低；并且由于采用视觉测距方案，因此在机器人视觉范围内，仅需要部署一个路标点，路标数量较少，从而使得室内定位的部署成本降低。

Description

室内定位方法和系统

技术领域

本发明涉及室内导航领域，特别涉及室内定位方法和系统。

背景技术

室内导航在许多领域都有广泛的应用，比如在室内移动机器人的自动路径引导、物流行业自动行驶小车(AGV小车)的自动行驶等。

针对移动机器人的室内导航定位方法很多，概括起来主要包括：航位推算法、绝对定位法和混合定位法。

其中，航位推算法是利用机器人装备的各种传感器获得当前机器人的动态位置信息，通过简单的递推累计公式获得机器人的估计位置，航位推算较常使用的传感器一般有码盘、陀螺仪、加速度计等。通过在机器人的轮子上安装码盘可以获得轮子转动的圈数，进而获得机器人相对于上一采样时刻位置和姿态的改变量，通过这些位移量的累积就可以估计机器人的位置。

绝对定位法是指机器人通过获得外界一些位置等已知的参照信息，通过计算自己与参照信息之间的相互关系进而解算出自己的位置。在室内环境下的绝对定位法较常使用的是路标定位法和模式匹配法等。路标定位法指机器人在运行过程中能够识别环境中固定位置的路标，通过解算相互之间的位置关系就可以计算机器人的位置。模式匹配法是指机器人使用各种传感器获得局部环境信息并与事先已知的全局环境信息相匹配，通过匹配关系获得自己在全局环境中的位置。

混合定位法则是将航位推算法和绝对定位法进行混合使用，以绝对定位法来补偿航位推算法的累积误差，从而提升定位精度。

在绝对定位法中常用的路标定位法中，常用的算法是三边测量法。三边测量法指移动机器人通过测量机器人距离三个或三个以上的路标点的距离信息来进行定位；通常的实施方案如下：在室内固定布置若干个路标点，路标点主动发射超声波、红外、无线信号等，移动机器人通过接收机接收到来自路标点的信息(超声信息、红外信息、无线信息等)，根据信号的强度或者到达时间来估算机器人和路标点的距离，然后通过三边测量法对机器人进行精确定位。该方案的缺点在于需要部署足够多的路标用于定位测量，路标需要主动发射无线信号(包括红外、超声、无线电等)，部署成本较高。

发明内容

本发明的目的在于提供一种室内定位方法和系统，使得室内定位的成本降低。

为解决上述技术问题，本发明的实施方式提供了一种室内定位方法，包含以下步骤：

对拍摄的图像进行预处理并提取特征；

将提取特征后的特征图像和预先建立的基于路标特征库的三维地图中的路标进行匹配，得到图像中可用于定位的路标；其中，所述路标为在室内的固定位置的视觉路标；

采用视觉测距方法测量当前位置与所述可用于定位的路标之间的距离；

通过视觉测距方法获取当前位置与所述可用于定位的路标之间的水平角度和垂直角度；

根据测量得到的距离、水平角度和垂直角度，计算出当前位置相对于所述识别出的路标的坐标。

本发明的实施方式还提供了一种室内定位系统，包含：图像获取模块、路标识别预处理模块、路标匹配模块、测距模块、位姿角度测量模块、定位主策略模块；

所述图像获取模块获取拍摄的图像；

所述路标识别预处理模块对所述图像获取模块获取的图像进行预处理并提取特征；

所述路标匹配模块将所述路标识别模块提取特征后的特征图像和预先建立的基于路标特征库的三维地图中的路标进行匹配，得到图像中可用于定位的路标；其中，所述路标为预先部署在室内的固定位置的视觉路标；所述路标特征库保存于预先建立的基于路标特征库的三维地图中；

所述测距模块采用视觉测距方案测量当前位置与所述可用于定位的路标之间的距离；

所述位姿角度测量模块通过视觉测距方法获取当前位置与所述可用于定位的路标之间的水平角度和垂直角度；

所述定位主策略模块根据所述测距模块测量得到的距离、所述位姿角度测量模块获取水平角度和垂直角度，计算出当前位置相对于所述识别出的路标的坐标。

本发明实施方式相对于现有技术而言，通过预先在室内的固定位置部署视觉路标(人工路标或者自然路标)，在定位过程中，移动机器人拍摄周边环境的图像，对拍摄的图像进行预处理和特征提取，将提取特征后的特征图像和路标特征库中的路标进行匹配，得到图像中可用于定位的路标；针对特征路标采用双目视觉测距方案对移动机器人进行实时定位。本发明的实施方式由于采用视觉路标(被动式路标)，因此路标本身不发射无线信号，路标本身成本较低；并且由于采用视觉测距方案，因此在机器人视觉范围内，仅需要部署一个路标点，路标数量较少，从而使得室内定位的成本降低。

另外，采用视觉测距和路标视觉识别方法建立所述基于路标特征库的三维地图，可以自动建立三维地图，适用于室内面积较大的复杂场景。

另外，也可以采用人工测绘方法建立所述基于路标特征库的三维地图，可以人工建立三维地图，适用于室内面积较小的简单场景。

另外，在移动机器人连续运动的过程中，移动机器人将连续对拍摄的图像进行分析处理，对已跟踪锁定的路标进行持续识别、跟踪，持续测量移动机器人当前所处位置与识别出的路标之间的距离、水平角度和垂直角度，不断计算出移动机器人当前所处位置相对于路标的坐标。采用这种路标自动跟踪的方法减少路标匹配的工作量，从而提高室内定位系统的实时性。

另外，所述图像中可用于定位的路标通过设置并计算路标权值的方法筛选得到。通过一定的筛选方法进行目标路标的筛选，可减少路标匹配的计算量，从而进一步提高室内定位系统的实时性。

附图说明

图1是根据本发明第一实施方式的室内定位方法的流程图；

图2是根据本发明第一实施方式的室内定位方法中路标特征提取算法的过程示意图；

图3是根据本发明第一实施方式的室内定位方法中设置并计算路标权值的方法的流程图；

图4是根据本发明第一实施方式的室内定位方法中路标自动跟踪方法的过程示意图；

图5是根据本发明第一实施方式的室内定位方法中机器人当前位置的绝对坐标的计算方法示意图；

图6是根据本发明第一实施方式的室内定位系统的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实施方式进行详细的阐述。然而，本领域的普通技术人员可以理解，在本发明各实施方式中，为了使读者更好地理解本申请而提出了许多技术细节。但是，即使没有这些技术细节和基于以下各实施方式的种种变化和修改，也可以实现本申请各权利要求所要求保护的技术方案。

本发明的第一实施方式涉及一种室内定位方法，如图1所示，具体包含以下步骤：

步骤101，安装于移动机器人的摄像头对外部环境进行拍摄。可以在移动机器人上部署可旋转的云台，云台上安装摄像头，采用摄像头拍摄图像。

步骤102，对拍摄的图像进行预处理并提取特征。

路标特征提取的算法采用通用的图像处理算法，对拍摄的图片进行处理和路标特征提取。

在本实施方式中，路标特征提取算法的基本过程包括：图像预处理、图像分割、图像特征提取过程，如图2所示，详细描述如下：

图像预处理用来解决移动机器人在前进过程中所拍摄的图片由于受光线、机身抖动、机身相对运动等原因导致的图像模糊不清、过曝光或欠曝光等问题，并消除图像中的无关信息、突出有用信息，增强信息的可处理性。图像预处理的基本方法包括灰度变换、灰度增强、边缘检测、直方图均衡、平滑、锐化和滤波处理，具体采用哪些方法主要根据后一过程不同的模式识别方式进行不同的预处理，一般来说需要综合和并行应用多种图像预处理技术，来得到不同的预处理结果，并进一步应用到后续的图像分割和特征提取过程。

图像分割是用来从复杂的拍摄图片中将包含感兴趣的路标物的内容区域提取出来，以便于后期的图像特性提取和模式识别过程更容易进行。根据路标物的不同特点，可选择性采用边缘、纹理、颜色、亮度等方式来实现图像的分割提取。图像分割的方法包括阈值分割和区域分割、边缘分割等等。

经过图像分割之后，需要从处理后的图像中进行特征的提取。图像的特征一般包括几何形状特征、颜色特征、亮度特征、纹理特征等等，具体采用何种特征提取方式由具体的模式识别方法来决定，一般来说，往往需要综合采用多种特征提取的手段来提高模式识别的准确性。

步骤103，将提取特征后的特征图像和预先建立的基于路标特征库的三维地图中的路标进行匹配，得到图像中可用于定位的路标；

其中，路标为在室内的固定位置的视觉路标。在实际应用中，可以在室内固定位置部署视觉识别路标，包括人工路标或者自然路标。人工路标是为了实现机器人定位而人为放置在机器人工作环境中的标识；自然路标是机器入易于识别的其运行环境中一些有明显特征且不会被移动或改变的自然物体，如门、窗、墙拐角等。

路标特征库保存于预先建立的基于路标特征库的三维地图中，可以采用视觉测距方法建立基于路标特征库的三维地图，这种方法可以自动建立三维地图，适用于室内面积较大的复杂场景。也可以采用人工测绘方法建立基于路标特征库的三维地图，这种方法可以人工建立三维地图，适用于室内面积较小的简单场景。

采用视觉测距方法建立基于路标特征库的三维地图包含以下步骤：

1)机器人位于原点位置。

2)安装在机器人固定位置的摄像头拍摄周边环境的图像；并对拍摄的图像进行预处理并提取特征。

3)对提取特征后的特征图像进行分析处理，通过A-SIFT算法和路标特征库中的路标进行匹配，识别出图像中存在的路标，并提取路标特征。

4)通过摄像头双目测距方法测量识别出的路标相对于机器人的位置；并以机器人原始位置为原点，绘制出该路标在三维地图的坐标。

5)机器人持续前进过程中，以惯性导航的前进速度和方向预测出之前识别出的路标在新拍摄图片中出现的位置。

6)通过上述步骤2～5对识别出的路标进行持续跟踪锁定。

7)通过摄像头双目测距方法测量识别出的路标相对于机器人当前位置相对于路标的相对坐标，修正机器人因为惯性导航带来的累积导航误差，得到较为精确的机器人当前坐标位置。

8)同时通过步骤2～5执行新路标的搜索匹配过程，并得到新匹配到的路标在三维地图中的地理位置信息。

9)移动机器人连续运动，重复上述步骤，不断测量识别出的路标的位置，建立包含多个路标的三维地图。

具体地说，在机器人的SLAM(Simultaneous Llocalization and Mapping，即时定位与地图构建)过程中，机器人首先通过摄像头在视觉范围内拍摄图像，并对拍摄的图像进行分析处理，识别出图像中存在的视觉路标。然后，移动机器人自动调整云台的方位角，使得摄像头对准视觉路标。此时，移动机器人将根据双目视觉测距测量到的距离、云台当前所调整的水平和垂直角度，通过三维方程式来计算出相当于路标的当前坐标位置。之后，在移动机器人连续运动的过程中，移动机器人将连续对拍摄的图像进行分析处理，对已跟踪锁定的路标根据持续识别、跟踪，持续调整云台的方位角，使得摄像头一直对准视觉路标。移动机器人根据双目视觉测距测量到的距离、云台当前所调整的水平和垂直角度，通过三维方程式来不断计算出相当于路标的当前坐标位置。同时，在移动机器人连续移动的过程中，会有旧的路标退出到机器人拍摄的画面之外，也会有新的被识别的路标进入到画面之中，路标库内容的连续更新将使得移动机器人可以绘制一幅连续的基于多个路标点的三维地图。

在车辆导航时，首先利用先验知识(比如预先将小车放置到特定的地图位置，然后再实时进行小车位置的跟踪)获得小车所在地图中的大致位置(非精确位置)，然后根据小车所在的大致位置下发需要进行特征匹配的“路标”供“路标匹配模块”进行特征匹配使用。选择的策略是：根据小车可能所处的大致地图位置，并结合路标筛选算法来选择合适的路标。图像中可用于定位的路标的选择方法有很多，本实施方式描述一种通过设置并计算路标权值的方法，如图3所示。

具体地说，设置单个路标的权重；其中，对不同场景中不同类型的路标设定不同的第一权重；对相同类型的路标，根据路标在图像中的大小，赋予不同的第二权重，面积越大，第二权重越大；如果同一区域存在多个相同类型的路标，则路标在图像中相隔的距离越近，设置的第三权重越小。

(1)设定第一权重，也就是，设置单个图标特征模型的权重。首先针对不同场景中不同类型的路标特征模板设定不同的权重，因为不同图标模型(比如墙角类路标、人工标牌等)对后续的特征匹配的运算量、准确率等影响度是不同的，因此希望对一些和背景区别较大、特征明显的物体设置更高的权重。比如说，如果一个场景中同时存在墙角和人工标牌类路标，可能更希望将人工标牌作为定位路标。

(2)设定第二权重。对于相同类别的路标模型来说，很多因素会影响特征匹配的有效性，如特征物体在成像图片中的大小等。需要针对不同的路标模型，明确具体的影响要素并对影响要素进行权重设置。比如说，拍摄的图片中包含有多个人工标牌，会希望选择图像更大一些的标牌作为路标物体，则需要对图像的面积赋予权重的设置，面积越大，权重越大。

(3)设定第三权重。针对相同类别的路标模型来说，需要考察是否存在多个相同类别路标集中于某个区域的情况。如果同一个区域存在多个相同类别的特征物体，这容易导致特征匹配时的误判断。因此，针对相同类别的特征物体在图像中的相隔距离进行权重的设置。距离越近，权重越小，以便更容易获得和周边物体明显具有区别的特征物体。比如说，一幅图像中存在有两个人工标牌靠在一起，则这情况下容易导致在匹配识别时混淆这两个人工标牌，因此通过设置权重的方式来减少取这两个标牌作为路标的可能。

在权重设置完毕之后，将每一个路标的第一权重、第二权重和第三权重相乘，得到所有路标的权值，从中筛选出权值最大的N个路标，作为当前位置可能存在的路标；其中，N为预设值。也就是说，将所有特征物体的权重进行计算(计算方法是将各个权重值进行相乘)，从而得到每个特征物体的权值，从中筛选出权重值最大的N个特征物体，作为后续用于定位匹配运算的特征路标。也就是说，在得到图像中可用于定位的路标的步骤中，如果拍摄图像中具有多个路标特征库的路标，则图像中可用于定位的路标通过设置并计算路标权值的方法筛选得到。

图像匹配算法不是本发明保护的重点，可以综合采用现有多种匹配算法，如仿射尺度不变特征变换(A-SIFT)算法或者霍夫变换(Hough)算法等等。比如采用A-SIFT算法将提取特征后的特征图像和预先建立的基于路标特征库的三维地图中的路标进行匹配，其中路标特征库定义了一组事先定义的特定路标模板的灰度图片，且这些路标模板的地理位置信息保存于预先建立的基于路标特征库的三维地图中。

为了提高匹配的准确度，本实施方式采用如下方法进行优化：

(1)采用多特征物体匹配方法，即，需要前面过程筛选出的N个特征路标均使用匹配算法进行匹配运算，并将所有匹配的结果根据三维空间的关联性(即特征物体在成像图片中的物理位置是固定的)进行相关运算，通过全局匹配的方法得出最大概率的匹配模板，减少单个特征物体匹配的误匹配率。

(2)采用路标自动跟踪的方法减少匹配的工作量。对路标的自动跟踪指的是：在移动机器人连续运动的过程中，移动机器人将连续对拍摄的图像进行分析处理，对已跟踪锁定的路标进行持续识别、跟踪，持续测量移动机器人当前所处位置与识别出的路标之间的距离、水平角度和垂直角度，不断计算出移动机器人当前所处位置相对于路标的坐标。

具体地说，识别到正确的路标之后，在机器人移动的过程中，系统将持续对该路标进行视觉锁定，通过调整云台的角度，将测距装置(和摄像头)持续对准路标。锁定过程中所采用图像处理方法也是通用的图像处理方法，和路标识别算法基本一致，如图4所示，包含：

●路标位置和大小预测：由于路标处于连续跟踪状态，因此目标物在视频图像中的变化(移动方位、大小变化)可以通过前面几帧的情况进行预测，减少进行后续图像分割处理的图像范围，而路标识别过程则是对整幅图片进行处理。

●图像预处理：和路标识别过程相同，该过程用来解决移动机器人在前进过程中所拍摄的图片由于受光线、机身抖动、机身相对运动等原因导致的图像模糊不清、过曝光或欠曝光等问题，并消除图像中的无关信息、突出有用信息，增强信息的可处理性。图像预处理的基本方法包括灰度变换、直方图均衡、平滑、锐化和滤波处理，具体采用哪些方法主要根据后一过程不同的模式识别方式进行不同的预处理，一般来说需要综合和并行应用多种图像预处理技术，来得到不同的预处理结果，并进一步应用到后续的图像分割和特征提取过程。

●图像分割：然后运用图像分割的方法进行路标物的区域提取，根据路标物的不同特点，可选择性采用边缘、纹理、颜色、亮度等方式来实现图像的分割提取。图像分割的方法包括阈值分割和区域分割、边缘分割等等。由于已经存在前若干帧的先验数据，因此在进行图像分割时可以充分利用先验数据来对算法的参数进行修正。

●特征提取和匹配：经过图像分割之后，需要从处理后的图像中进行特征的提取。图像的特征一般包括几何形状特征、颜色特征、亮度特征、纹理特征等等，具体采用何种特征提取方式由具体的模式识别方法来决定，一般来说往往需要综合采用多种特征提取的手段来提高模式识别的准确性。

●质心位置计算：在匹配完成之后，需要根据路标物的边缘闭合区间进行质心的位置计算。

●调整云台使得摄像头装置对准路标质心位置。

在得到图像中可用于定位的路标，也就是将测距装置对准目标路标之后，执行步骤104至106，采用双目视觉测距方案测量移动机器人当前位置与目标路标(图像中可用于定位的路标对应在室内的实物)之间的距离，从而确定移动机器人当前位置的坐标。

具体地说，步骤104，采用视觉测距方法测量当前位置与可用于定位的路标之间的距离；

步骤105，通过视觉测距方法获取当前位置与可用于定位的路标之间的水平角度和垂直角度；

在实际应用中，可以在移动机器人上部署可旋转的云台，云台上安装摄像头，用于测量移动机器人与路标之间的距离；同时在移动机器人上选择性部署用于测量水平方位的电子罗盘和测量垂直方位的陀螺仪，用于获取云台当前所调整的水平和垂直角度，也就是，当前位置与可用于定位的路标之间的水平角度和垂直角度。

步骤106，根据测量得到的距离、水平角度和垂直角度，计算出当前位置相对于识别出的路标的坐标。计算方法如图5所示，具体说明如下：

●以目标路标的质心为原点，以正北方向为X轴，以正东方向为Y轴，以垂直向下为Z轴建立三维坐标系。

●假设云台(根据摄像头的视角)的初始方位角为水平方向(相对于正南方向)为α，相对于垂直方向(相对于垂直向上方向)的角度为β。假设云台调整的角度为水平方向(相对于初始位置顺时针方向)为α'，垂直方向(相对于初始位置顺时针方向)为β'，测得云台距离原点的距离为D。

●可以计算出云台位置的三维坐标(x,y,z)分别为：

x＝D·sin(β+β')·cos(α+α')

y＝D·sin(β+β')·sin(α+α')

z＝D·cos(β+β')

简言之，在机器人建立完基于特征路标的地图之后的自动行驶过程中，机器人首先通过摄像头在视觉范围内拍摄图像，并对拍摄的图像进行分析处理，通过图像匹配算法将当前位置可能存在的路标和拍摄的图像进行匹配，识别出图像中存在的视觉路标。然后，移动机器人自动调整云台的方位角，使得摄像头对准视觉路标。此时，移动机器人将根据双目视觉测量到的距离、云台当前所调整的水平和垂直角度，通过三维方程式来计算出相当于路标的当前坐标位置。之后，在移动机器人连续运动的过程中，移动机器人将连续对拍摄的图像进行分析处理，对已跟踪锁定的路标根据持续识别、跟踪，持续调整云台的方位角，使得一直对准视觉路标。移动机器人根据双目测距测量到的距离、云台当前所调整的水平和垂直角度，通过三维方程式来不断计算出相当于路标的当前坐标位置。同时，在移动机器人连续移动的过程中，会有旧的路标退出到机器人拍摄的画面之外，也会有新的被识别的路标进入到画面之中，路标库内容的连续更新将使得移动机器人可以连续进行位置信息的定位。

针对传统方案的不足，本实施方式提出的方案使用视觉识别加双目测距方法的方法建立基于路标库的三维地图，然后在车辆行驶的动态过程中，通过视觉识别的方式自动将路标库中的路标和实拍的图片进行特征匹配，然后自动调整摄像头对准准确匹配的路标物体进行测距，通过针对特征路标的双目测距技术对车辆进行实时定位，其独特点包括：

(1)基于路标的导航和定位方式，采用视觉路标(被动式路标)，路标本身不发射无线信号。

(2)对视觉路标进行测距时，采用双目视觉测距方案，即在机器人视觉范围内，最少仅需要部署一个路标点。

(3)通过对视觉路标的图像识别技术来实现移动机器人对视觉路标的精确识别和锁定，并将视觉识别技术和双目测距技术进行融合来实现基于路标的绝对定位。

即，系统自动识别路标，然后自动控制云台对准路标的定位点，让双目测距方式进行测距。

(4)在机器人运动的过程中同样通过对视觉路标的图像识别技术自动跟踪路标，并通过对机器人运动轨迹的测量来对已锁定的视觉路标在成像图片中的变化趋势进行预测，减少跟踪锁定路标的计算工作量。

上面各种方法的步骤划分，只是为了描述清楚，实现时可以合并为一个步骤或者对某些步骤进行拆分，分解为多个步骤，只要包含相同的逻辑关系，都在本专利的保护范围内；对算法中或者流程中添加无关紧要的修改或者引入无关紧要的设计，但不改变其算法和流程的核心设计都在该专利的保护范围内。

本发明第二实施方式涉及一种室内定位系统，如图6所示，包含：图像获取模块、路标识别预处理模块、路标匹配模块、测距模块、位姿角度测量模块、定位主策略模块。

在室内固定位置部署视觉识别路标(包括人工路标或者自然路标)。人工路标是为了实现机器人定位而人为放置在机器人工作环境中的标识；自然路标是机器入易于识别的其运行环境中一些有明显特征且不会被移动或改变的自然物体，如门、窗、墙拐角等。

在移动机器人上部署可旋转的云台，云台上安装摄像头；同时在移动机器人上选择性部署用于测量水平方位的电子罗盘和测量垂直方位的陀螺仪。

图像获取模块获取拍摄的图像，也就是，图像获取模块完成对周边环境的图像拍摄，可以采用摄像头来实现。拍摄后的图像数据传递给路标识别模块或者定位主策略模块使用。

路标识别预处理模块对图像获取模块获取的图像进行预处理并提取特征。路标识别模块对来自于图像获取模块的图像数据进行处理分析，对图像进行预处理并进行特征提取后，将提取后的特征图像和来自定位主策略模块发来的路标特征库进行匹配，一旦完成匹配，表示该图像中存在可用于定位的路标。路标识别模块将处理后的结果(包括特征物、特征物在图像中的位置和大小)信息传递给定位主策略模块使用。该模块可采用嵌入式软件方式实现，可采用CPU或者DSP、专用芯片来完成图像数据的处理过程。

路标匹配模块将路标识别模块提取特征后的特征图像和预先建立的基于路标特征库的三维地图中的路标进行匹配，得到图像中可用于定位的路标；其中，路标为在室内的固定位置的视觉路标。路标匹配模块在定位主策略模块的控制下，完成将来自于图像获取模块的图像数据和路标库进行图像匹配的运算，并将匹配的结果反馈给定位主策略模块使用。该模块可采用嵌入式软件方式实现，可采用CPU或者DSP、专用芯片来完成图像数据的处理过程。

测距模块采用视觉测距方法测量当前位置与可用于定位的路标之间的距离。测距模块完成移动机器人和路标之间的距离测量，当路标被明确后，通过本测距模块完成移动机器人和路标之间的距离测定。本方案中可以采用双目测距的方法，但不排除使用其他测距的手段，如红外和激光测距、声呐测距等。测距模块接收来自定位主策略模块的命令进行测距，并将测量结果反馈给定位主策略模块。

位姿角度测量模块通过视觉测距方法获取当前位置与可用于定位的路标之间的水平角度和垂直角度。位姿角度测量模块完成对移动机器人的云台基础角度的测量，即云台相对于水平方向和垂直方向的偏置角度。可以采用电子罗盘和陀螺仪来实现。位姿角度测量模块测量的结果(角度数据)实时传递给定位主策略模块使用。

云台和云台控制模块包括云台和云台控制部分，云台是安装、固定摄像机(和测距模块)的支撑设备，电动云台的姿态调整是由两台电动机来实现，电动机接受来自云台控制部分的信号精确地运行定位。云台和云台控制模块接收来自定位主策略模块的控制信息，并根据控制信息执行云台的旋转动作，将云台的调整角度反馈给定位主策略模块。

定位主策略模块根据测距模块测量得到的距离、位姿角度测量模块获取水平角度和垂直角度，计算出当前位置相对于识别出的路标的坐标。“定位主策略模块”是本专利的核心模块，完成定位的策略选择、匹配路标选择和移动机器人绝对位置的计算。该模块可采用嵌入式软件方式实现，也可采用CPU或者DSP、专用芯片来完成图像数据的处理过程。

定位主策略模块包含以下子模块：权重设置子模块和路标筛选子模块；

权重设置子模块用于设置单个路标的权重；其中，对不同场景中不同类型的路标设定不同的第一权重；对相同类型的路标，根据路标在图像中的大小，赋予不同的第二权重，面积越大，第二权重越大；如果同一区域存在多个相同类型的路标，则路标在图像中相隔的距离越近，设置的第三权重越小；

路标筛选子模块用于将每一个路标的第一权重、第二权重和第三权重相乘，得到所有路标的权值，从中筛选出权值最大的N个路标，作为当前位置可能存在的路标；其中，N为预设值。

采用本实施方式的室内定位系统建立基于路标特征库的三维地图的基本过程如下：

在机器人移动的过程中拍摄图片，图像获取模块完成对周边环境的图像拍摄，图片中包含室内常见的墙角、开关、灯、门框、立柱或一些人工设置的标牌路标；拍摄后的图像数据传递给路标识别模块，路标识别模块通过对图片的图像处理的手段进行特征物体的特征提取。然后，主策略模块通过控制云台和云台控制模块调整云台，通过双目测距方法对这些提取的特征路标进行位置测量来建立包含多个特征模板的三维地图，并给出这些特征模板在地图中的位置。

也就是说，定位主策略模块还用于对基于路标特征库的三维地图的建立进行控制。在建立基于路标特征库的三维地图过程中，安装在机器人固定位置的摄像头作为图像获取模块拍摄周边环境的图像。路标识别预处理模块对图像获取模块拍摄的图像进行预处理并提取特征。路标匹配模块用于对提取特征后的特征图像进行分析处理，通过A-SIFT算法和路标特征库中的路标进行匹配，识别出图像中存在的路标，并提取路标特征。测距模块和位姿角度测量模块通过摄像头双目测距方法测量识别出的路标相对于机器人的位置。定位主策略模块还用于以机器人原始位置为原点，绘制出该路标在三维地图的坐标。

此外，室内定位系统还包含：路标预测模块、跟踪锁定模块和误差修正模块。路标预测模块用于机器人持续前进过程中，以惯性导航的前进速度和方向预测出之前识别出的路标在新拍摄图片中出现的位置。跟踪锁定模块用于控制路标识别预处理模块、路标匹配模块、测距模块和位姿角度测量模块对识别出的路标进行持续跟踪锁定。误差修正模块用于通过摄像头双目测距方法测量识别出的路标相对于机器人当前位置相对于路标的相对坐标，修正机器人因为惯性导航带来的累积导航误差，得到较为精确的机器人当前坐标位置。定位主策略模块还用于通过图像获取模块、路标识别预处理模块、路标匹配模块、测距模块和位姿角度测量模块执行新路标的搜索匹配过程，并得到新匹配到的路标在三维地图中的地理位置信息。移动机器人连续运动，通过上述所有模块不断测量识别出的路标的位置，建立包含多个路标的三维地图。

采用本实施方式的室内定位系统实现导航过程中的实时定位的基本过程如下：

机器人前进过程中，图像获取模块完成对周边环境的图像拍摄，拍摄后的图像数据传递给主策略模块，主策略模块根据小车所在地图中的大致位置从基于路标特征库的三维地图中选择若干个用于定位的路标，并使用匹配算法对这些路标和预处理后的的图像数据进行匹配，在匹配成功后，定位主策略模块控制云台和云台控制模块调整云台角度，使得激光测距模块能够对准检测出的匹配路标进行测距，测量出机器人当前的物理位置，测距模块将测量结果反馈给定位主策略模块。在完成路标的匹配之后，定位主策略模块控制云台和云台控制模块进行所检出路标的动态自动跟踪。

不难发现，本实施方式为与第一实施方式相对应的系统实施例，本实施方式可与第一实施方式互相配合实施。第一实施方式中提到的相关技术细节在本实施方式中依然有效，为了减少重复，这里不再赘述。相应地，本实施方式中提到的相关技术细节也可应用在第一实施方式中。

值得一提的是，本实施方式中所涉及到的各模块均为逻辑模块，在实际应用中，一个逻辑单元可以是一个物理单元，也可以是一个物理单元的一部分，还可以以多个物理单元的组合实现。此外，为了突出本发明的创新部分，本实施方式中并没有将与解决本发明所提出的技术问题关系不太密切的单元引入，但这并不表明本实施方式中不存在其它的单元。

本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。

Claims (6)

1.一种室内定位方法，其特征在于，包含以下步骤：

对拍摄的图像进行预处理并提取特征；

将提取特征后的特征图像和预先建立的基于路标特征库的三维地图中的路标进行匹配，得到图像中可用于定位的路标；其中，所述路标为在室内的固定位置的视觉路标；

采用视觉测距方法测量当前位置与所述可用于定位的路标之间的距离；

通过视觉测距方法获取当前位置与所述可用于定位的路标之间的水平角度和垂直角度；

根据测量得到的距离、水平角度和垂直角度，计算出当前位置相对于所述可用于定位的路标的坐标；

其中，在移动机器人连续运动的过程中，移动机器人将连续对拍摄的图像进行分析处理，对已跟踪锁定的路标进行持续识别、跟踪，持续测量移动机器人当前所处位置与识别出的路标之间的距离、水平角度和垂直角度，不断计算出移动机器人当前所处位置相对于路标的坐标；

在得到图像中可用于定位的路标的步骤中，如果拍摄图像中具有多个路标特征库的路标，则所述图像中可用于定位的路标通过设置并计算路标权值的方法筛选得到；

所述设置并计算路标权值的方法包含以下子步骤：

设置单个路标的权重；其中，对不同场景中不同类型的路标设定不同的第一权重；对相同类型的路标，根据路标在图像中的大小，赋予不同的第二权重，面积越大，第二权重越大；如果同一区域存在多个相同类型的路标，则路标在图像中相隔的距离越近，设置的第三权重越小；

将每一个路标的第一权重、第二权重和第三权重相乘，得到所有路标的权值，从中筛选出权值最大的N个路标，作为所述当前位置存在的路标；其中，所述N为预设值。

2.根据权利要求1所述的室内定位方法，其特征在于，采用视觉测距和路标视觉识别方法建立所述基于路标特征库的三维地图。

3.根据权利要求2所述的室内定位方法，其特征在于，所述三维地图的建立方法包含以下步骤：

S1.机器人位于原点位置；

S2.安装在机器人固定位置的摄像头拍摄周边环境的图像；并对拍摄的图像进行预处理并提取特征；

S3.对提取特征后的特征图像进行分析处理，通过A-SIFT算法和路标特征库中的路标进行匹配，识别出图像中存在的路标，并提取路标特征；

S4.通过摄像头双目测距方法测量所述识别出的路标相对于机器人的位置；并以机器人原始位置为原点，绘制出该路标在三维地图的坐标；

S5.机器人持续前进过程中，以惯性导航的前进速度和方向预测出之前识别出的路标在新拍摄图片中出现的位置；

S6.通过上述步骤S2～S5对识别出的路标进行持续跟踪锁定；

S7.通过摄像头双目测距方法测量所述机器人当前位置相对于路标的相对坐标，修正机器人因为惯性导航带来的累积导航误差，得到较为精确的机器人当前坐标位置；

S8.同时通过步骤S2～S5执行新路标的搜索匹配过程，并得到新匹配到的路标在三维地图中的地理位置信息；

S9.移动机器人连续运动，不断测量识别出的路标的位置，建立包含多个路标的三维地图。

4.根据权利要求1所述的室内定位方法，其特征在于，在所述将提取特征后的特征图像和预先建立的基于路标特征库的三维地图中的路标进行匹配的步骤中，通过仿射尺度不变特征变换A-SIFT算法进行匹配。

5.一种室内定位系统，其特征在于，包含：图像获取模块、路标识别预处理模块、路标匹配模块、测距模块、位姿角度测量模块、定位主策略模块；

所述图像获取模块获取拍摄的图像；

所述路标识别预处理模块对所述图像获取模块获取的图像进行预处理并提取特征；

所述路标匹配模块将所述路标识别预处理模块提取特征后的特征图像和预先建立的基于路标特征库的三维地图中的路标进行匹配，得到图像中可用于定位的路标；其中，所述路标为在室内的固定位置的视觉路标；

所述测距模块采用视觉测距方法测量当前位置与所述可用于定位的路标之间的距离；

所述位姿角度测量模块通过视觉测距方法获取当前位置与所述可用于定位的路标之间的水平角度和垂直角度；

所述定位主策略模块根据所述测距模块测量得到的距离、所述位姿角度测量模块获取水平角度和垂直角度，计算出当前位置相对于所述可用于定位的路标的坐标；

其中，在移动机器人连续运动的过程中，移动机器人将连续对拍摄的图像进行分析处理，对已跟踪锁定的路标进行持续识别、跟踪，持续测量移动机器人当前所处位置与识别出的路标之间的距离、水平角度和垂直角度，不断计算出移动机器人当前所处位置相对于路标的坐标；

在得到图像中可用于定位的路标的步骤中，如果拍摄图像中具有多个路标特征库的路标，则所述图像中可用于定位的路标通过设置并计算路标权值的方法筛选得到；

所述定位主策略模块包含以下子模块：权重设置子模块和路标筛选子模块；

所述权重设置子模块用于设置单个路标的权重；其中，对不同场景中不同类型的路标设定不同的第一权重；对相同类型的路标，根据路标在图像中的大小，赋予不同的第二权重，面积越大，第二权重越大；如果同一区域存在多个相同类型的路标，则路标在图像中相隔的距离越近，设置的第三权重越小；

所述路标筛选子模块用于将每一个路标的第一权重、第二权重和第三权重相乘，得到所有路标的权值，从中筛选出权值最大的N个路标，作为所述当前位置存在的路标；其中，所述N为预设值。

6.根据权利要求5所述的室内定位系统，其特征在于，所述定位主策略模块还用于对基于路标特征库的三维地图的建立进行控制；

在建立基于路标特征库的三维地图过程中，安装在机器人固定位置的摄像头作为所述图像获取模块拍摄周边环境的图像；

所述路标识别预处理模块对所述图像获取模块拍摄的图像进行预处理并提取特征；

所述路标匹配模块用于对提取特征后的特征图像进行分析处理，通过A-SIFT算法和路标特征库中的路标进行匹配，识别出图像中存在的路标，并提取路标特征；

所述测距模块和所述位姿角度测量模块通过摄像头双目测距方法测量所述识别出的路标相对于机器人的位置；

所述定位主策略模块还用于以机器人原始位置为原点，绘制出该路标在三维地图的坐标；

所述室内定位系统还包含：路标预测模块、跟踪锁定模块和误差修正模块；

所述路标预测模块用于机器人持续前进过程中，以惯性导航的前进速度和方向预测出之前识别出的路标在新拍摄图片中出现的位置；

所述跟踪锁定模块用于控制所述路标识别预处理模块、路标匹配模块、测距模块和位姿角度测量模块对识别出的路标进行持续跟踪锁定；

所述误差修正模块用于通过摄像头双目测距方法测量所述机器人当前位置相对于路标的相对坐标，修正机器人因为惯性导航带来的累积导航误差，得到较为精确的机器人当前坐标位置；

所述定位主策略模块还用于通过所述图像获取模块、路标识别预处理模块、路标匹配模块、测距模块和位姿角度测量模块执行新路标的搜索匹配过程，并得到新匹配到的路标在三维地图中的地理位置信息；

移动机器人连续运动，通过上述所有模块不断测量识别出的路标的位置，建立包含多个路标的三维地图。