CN105953798A - 移动机器人的位姿确定方法和设备 - Google Patents

Abstract

本发明的实施方式涉及移动机器人的位姿确定方法和设备。该方法包括：在移动机器人的位姿发生改变时，利用惯性导航传感器计算移动机器人在全局地图中的第一位姿；使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于移动机器人的周围环境的局部地图；将局部地图在全局地图中进行图像匹配，获取所述移动机器人在全局地图中的第二位姿和匹配可信度，其中所述第二位姿为图形匹配得出的所述移动机器人在所述全局地图中的最佳位姿；以及根据匹配可信度，确定移动机器人的位姿为第一位姿或者第二位姿。根据本发明的实施方式，可以改善移动机器人位姿的确定，减少位姿确定时的误差。

Description

移动机器人的位姿确定方法和设备

技术领域

本发明的实施方式涉及移动机器人定位和导航的技术领域，特别涉及移动机器人导航中的位姿确定。

背景技术

诸如家庭服务机器人的移动机器人可以在室内自主行走，首先必须能够对其精确定位。室内定位和导航目前出现了各种技术，如电磁线、磁带线、超宽带技术、基于无线局为(WLAN)定位技术、激光测距定位技术、基于惯性导航的定位技术、基于里程计的定位技术等。而每种定位技术都存在各自的缺点和局限性，大多数方案采用多种定位技术的组合导航。

目前电磁线与磁带线导航方案较为成熟，导航准确度高，目前自动导向车(AGV；Automatic Guided Vehicle)和餐饮机器人已经在成功使用此两类方案。这两种方案原理简单且可靠，但却存在较大的共同缺点，必须按照已铺设好的路线导航。尤其电磁线方案，再次更改路线时十分困难。因此不适合自由行走，灵活多变的家庭服务机器人。

Wifi、蓝牙、超宽带等无线室内导航策略目前已较为成熟，Wifi、蓝牙由于定位精度低目前还不能应用于的室内自主导航机器人身上。同时无线导航传感器均受墙反射影响，在空间狭小的家庭居室中精度及其下降，不适用于自主运行的家庭服务机器人。并且需提前对环境进行较大改造和标定无线定位节点坐标。

惯性导航通过陀螺仪、加速计、里程计等传感器进行定位，定位精度高满足室内定位要求，但累计误差无法避免，且需要提前绘制室内结构地图。

激光导航目前有两种策略：1.在每个室内安装高反光板材料，激光扫描角度范围至少存在三个，通过三角定位获取位姿，精度高。缺点是需改造环境，同时标定反光板位置，缺少通用性。同时反光板可能会变脏、变形、脱落等因素导致定位错误。2.采用即时定位与地图构建(SLAM)技术进行及时地图创建和定位，准确度高，通用性高。再次开机调取第一次所创地图进行匹配定位，从而完成绝对定位。但地图匹配受外界环境因素较大，当环境发生临时变动时，定位误差增加甚至定位错误。

可见，由于每种定位技术所依赖的传感器本身精度限制、以及室内环境复杂、空间狭小等因素，导致室内定位不可避免地存在位置和航向角误差。因此，移动机器人的位姿(也称为定位)的准确度直接影响到室内导航的实用性。

发明内容

有鉴于此，本发明实施方式的所要解决的技术问题之一是改善移动机器人的位姿确定。

根据本发明的一个方面，提供一种移动机器人的位姿确定方法。所述移动机器人包括激光扫描仪和惯性导航传感器。该方法可以包括：在所述移动机器人的位姿发生改变时，利用所述惯性导航传感器计算所述移动机器人在全局地图中的第一位姿；使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于所述移动机器人的周围环境的局部地图；将所述局部地图在全局地图中进行图像匹配，获取所述移动机器人在所述全局地图中的第二位姿、和与所述第二位姿对应的匹配可信度，其中所述第二位姿为图形匹配得出的所述移动机器人在所述全局地图中的最佳位姿，并且其中所述全局地图是使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建的关于所述移动机器人在其中导航的环境的地图；以及根据所述匹配可信度，确定所述移动机器人的位姿为所述第一位姿或者所述第二位姿。

根据本发明的一个方面，提供一种移动机器人的位姿确定设备，所述移动机器人包括激光扫描仪和惯性导航传感器。该设备可以包括：第一位姿确定装置，用于在所述移动机器人的位姿发生改变时，利用所述惯性导航传感器计算所述移动机器人在全局地图中的第一位姿；局部地图创建装置，用于使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于所述移动机器人的周围环境的局部地图；第二位姿确定装置，用于将所述局部地图在全局地图中进行图像匹配，获取所述移动机器人在所述全局地图中的第二位姿、和与所述第二位姿对应的匹配可信度，其中所述第二位姿为图形匹配得出的所述移动机器人在所述全局地图中的最佳位姿，并且其中所述全局地图是使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建的关于所述移动机器人在其中导航的环境的地图；以及最终位姿确定装置，用于根据所述匹配可信度，确定所述移动机器人的位姿为所述第一位姿或者所述第二位姿。

根据本发明的一个方面，提供一种在计算机可读存储介质中实现的计算机程序产品，所述计算机可读存储介质具有存储于其中的计算机可读程序代码部分，所述计算机可读程序代码部分被配置为执行根据前述的方法。

根据本发明实施方式的技术方案，提供了基于激光扫描仪和惯性导航传感器的组合导航方法，在已有全局地图中激光导航可以修正惯性导航带来的累计误差，惯性导航可以修正外界环境临时变动造成激光导航策略定位错误。根据本发明实施方式，当机器人处于新环境中时可以自动创建全局地图，从而无需对环境进行任何改造，也无需提供室内平面图，适用性强，灵活性高，紧靠机器人自身传感器可以完成自主导航。

从下文结合附图所做出的详细描述中，本发明的这些和其他优点和特征将变得更加明显，其中在整个下文描述的若干附图中，类似的元件将具有类似的编号。

附图说明

图1图示根据本发明的一个实施方式的移动机器人位姿确定方法的流程图；

图2图示根据本发明的一个实施方式的移动机器人位姿确定方法的流程图；

图3图示根据本发明的一个实施方式的全局地图创建和移动机器人位姿初始化方法的流程图；

图4图示根据本发明一个实施例所创建的全局地图；

图5图示根据本发明一个实施例所创建的局部地图；

图6图示如图5所示的局部地图在如图4所示的全局地图中的匹配示意图；

图7图示根据本发明实施方式的一种模块匹配方法的示意图；

图8图示如图5所示的局部地图中的最小有效地图图像的示意图；

图9图示根据本发明实施方式的一种激光导航测量原理结构图；

图10图示移动机器人所包括的激光扫描仪的一种可能布置的示意图；

图11图示移动机器人所包括的激光扫描仪的另一种可能布置的示意图；

图12图示根据本发明的一个实施方式的移动机器人位姿确定方法的流程图；

图13图示现有技术的室内定位和导航技术的方法的流程图；

图14图示了基于超宽带(UWB)的定位和导航技术的示意图；

图15图示根据本发明的实施方式的用于移动机器人定位的方法的流程图；

图16图示根据本发明的实施方式的用于移动机器人定位和导航的方法的流程图；

图17A、图17B和图17C图示实际参考物图像相对于数据库参考物图像所可能发生变化的示意图；

图18图示实际参考物图像与数据库参考物图像匹配的示意图；以及

图19图示了移动机器人拍摄画面中的数据库参考物图像和实际参考物图像的可能的偏差。

具体实施方式

下文将参考附图更完整地描述本公开内容，其中在附图中显示了本公开内容的实施方式。但是这些实施方式可以用许多不同形式来实现并且不应该被解释为限于本文所述的实施方式。相反地，提供这些实例以使得本公开内容将是透彻和完整的，并且将全面地向本领域的熟练技术人员表达本公开内容的范围。

参考图1，其图示了图根据本发明的一个实施方式的移动机器人位姿确定方法100的流程图。移动机器人包括激光扫描仪和惯性导航传感器。

在步骤S110，在移动机器人的位姿发生改变时，利用惯性导航传感器计算移动机器人在全局地图中的第一位姿。全局地图可以是针对移动机器人在其中移动的环境而预先创建的。使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术、针对移动机器人在其中导航的环境而预先创建的。

惯性导航传感器可以包括陀螺仪、电子指南针、加速计和里程计中一种或多种。陀螺仪获得机器人可以旋转角度，加速计或里程计可以获取行走距离，电子指南针可以获取机器人绝对航向角。惯性导航的基本原理为：设移动机器人t时刻位姿为(x_t，y_t，θ_t)，例如采用陀螺仪和里程计传感器获得角度变化信息、前行距离，可以推出t+1时刻的测量位姿(x_t+1，y_t+1，θ_t+1)。

在步骤S120，使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于所述移动机器人的周围环境的局部地图。

在步骤S130，将局部地图在全局地图中进行图像匹配，获取移动机器人在全局地图中的第二位姿、和与第二位姿对应的匹配可信度。第二位姿为图形匹配得出的移动机器人在全局地图中的最佳位姿。可以采用的具体图像匹配算法可以包括但不限于，尺度不变特征转换(SIFT，Scale-invariantfeature transform)算法和模板匹配算法，还可以使用直方图匹配、形状匹配、蒙特卡洛等匹配算法。

在步骤S140，根据所获取的匹配可信度，确定移动机器人的位姿为第一位姿或者第二位姿。匹配可信度表示第二位姿的准确度，并且其值越大，表明第二位姿越精确，从而有，当所获取的匹配可信度大于第一阈值时，确定移动机器人的位姿为第二位姿，并且当所获取的匹配可信度小于或者等于第一阈值时，确定移动机器人的位姿为第一位姿。

参考图2，其图示了根据本发明的一个实施方式的移动机器人位姿确定方法200的流程图。移动机器人包括激光扫描仪和惯性导航传感器。

在步骤S210，判断移动机器人是否首次出现在新环境中，若判断结果为是，则方法200前进至步骤S220，否则，方法200前进至步骤S230。

在步骤S220，使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建全局地图，并且在移动机器人上电时对移动机器人在该全局地图中的位姿进行初始化。更多的细节将在下文参考图3进行描述。

后续步骤S230、S240、S250和S260涉及在移动机器人的位姿发生改变时，确定移动机器人在全局地图中的精确位姿，其分别与参考方法100所描述的步骤S110、S120、S130和S140相同。

惯性导航定位技术和激光导航定位技术是目前较为成熟的几种室内导航定位手段中的两种手段，二者都需要依赖于预先设定的关于环境的全局地图。另外，惯性导航定位精度高，但是存在累计误差，上电初始时刻，需配合外部其他设备提供地图和初始位姿。基于激光扫描仪的即时定位与地图构建(SLAM)技术可在新环境下同时建图和定位，第一次创建地图时，初始位姿可为固定值或由其他设备提供，但是在完成创建地图后，上电初始时刻无法确定移动机器人在已有地图中的初始位姿。

参考图3，其图示了根据本发明的一个实施方式的全局地图创建和移动机器人位姿初始化方法300的流程图。

在步骤S310，使用激光扫描仪，利用即时定位与地图构建(SLAM)技术(下文也称为激光SLAM技术)创建移动机器人在其中导航的环境的全局地图。移动机器人在导航环境中的位姿包括坐标和航向角，用(x，y，θ)来表示。

在一个未知环境(比如室内环境)下，采用激光SLAM技术对当前环境进行第一次建图，可以设定地图大小(地图大小应比实际的室内范围大)为2048*2048。建图时移动机器人的假定初始位姿比如可固定为(1024,1024,0)。可以自主地导航移动机器人使其走遍室内范围，也可以人工引导机器人走遍整个室内范围，创建全局地图。创建的地图可以为栅格地图，如图4所示，其示出了作为一个实施例的所创建的全局地图，其中障碍的像素值为0，无障碍的像素值为255，未观测到的像素值为127。可以将创建好的地图进行保存在移动机器人的存储器中，或者移动机器人外部的存储设备中、在后续的步骤中移动机器人可以与该外部存储设备中进行无线或有线通信来获取所保存的全局地图。

在建立了上述地图的情况下，则此后移动机器人的定位、导航、路径规划等均可以以此地图为全局地图，定位点以此图的坐标为绝对位姿。

在步骤S320，在移动机器人上电时，使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建针对移动机器人的上电时刻的环境的局部地图。

在移动机器人上电重新启动时，可以设置移动机器人的假定初始位姿为(1024,1024,0)，开启激光SLAM建图功能，使得移动机器人在原位置自行旋转一周，形成较大的、详细的、360度的局部地图，如图5所示，其示出了作为一个实施例的所创建的局部地图。由于移动机器人首次出现在新环境中、或者上电时刻移动机器人位姿可能为任意值，故匹配时需考虑360度范围的航向角情况下在地图中的位置。应当理解，要创建360度的局部地图，需要的移动机器人旋转的角度和移动机器人上布置的激光扫描仪数量以及激光扫描仪视野范围有关。

在步骤S330，将局部地图在全局地图中进行图像匹配，获取移动机器人在全局地图中的初始位姿。可以将移动机器人上电重新启动时所创建的360度的局部地图与全局地图采用图像处理方法进行图像匹配，得到其在全局地图中的位置和方向，如图6所示，其示出了如图5所示的局部地图在如图4所示的全局地图中的匹配示意图。从而，根据图像匹配的结果，可以获取移动机器人的在全局地图中的初始位姿，其为绝对位姿。

激光SLAM所创建的地图可以为二维栅格灰度图像，可如图4、图5所示，地图存在复杂性、毛刺、粗糙、不工整等特点，尤其此类二维图像的特征点不是十分明显。同时在创建局部底部的初始时刻，激光所测局部距离信息点为离散的、局部性强、特征点残缺等特点，直接与原始的全局地图进行匹配易出现匹配错误或无法得到正确的匹配。进一步地，为了实现局部地图与全局地图的精准匹配，可以采用的具体图像匹配方式包括但不限于尺度不变特征转换(SIFT，Scale-invariant featuretransform)算法和模板匹配算法，还可以使用直方图匹配、形状匹配、蒙特卡洛等匹配算法。

SIFT算法是用于图像匹配的经典算法之一，用来侦测与描述影像中的局部性特征，它在空间尺度中寻找极值点，并提取出其位置、尺度、旋转不变量。此算法由David Lowe在1999年所发表，2004年完善总结。根据本发明的实施方式，局部地图和全局地图采用完全相同的SLAM算法绘制而成，从图4、图5、图6可以看出，图4所示局部地图是图3所示的全局地图中的一部分，但是位置和朝向随机(由于全局地图是扫描仪的n次扫描结果相叠加的结果，而局部地图是1次扫描的结果，所以肉眼未必能够发现局部地图在全局地图中的位置和朝向，但计算机视觉算法SIFT可以实现)。故图像处理中SIFT算法非常适用于旋转、平移的两幅图像匹配，即本发明实施方式的场景。图5相对于图4进行SIFT特征提取、并进行匹配，得出旋转角度为θ_t、平移向量为(x_t，y_t)时为最佳匹配和匹配可信度，如图6所示。由于两幅图建图时的移动机器人假定初始位姿相同，比如均为(1024,1024,0)，而其中图4作为绝对坐标系，图5中的假定初始位姿(1024,1024,0)实际为图4中当前位姿，经旋转与平移即可得到在图4中的绝对坐标。从而，获取移动机器人在图4的全局地图中的初始位姿POSE为：

POSE＝(1024,1024,0)+(x_t，y_t，θ_t)

＝(1024+x_t，1024+y_t，θ_t)。

模板匹配也是用于图像匹配的经典算法之一，用于判断数字图像的模板是否匹配，即模板与原图像进行比较，以确定在原图像中是否存在与该模板相同或相似的区域，若该区域存在，还可确定其位置，认为模板与原图像匹配。根据本发明的实施方式，局部地图和全局地图采用完全相同的SLAM算法绘制而成，从图4、图5、图6可以看出，图4所示局部地图是图3所示的全局地图中的一部分，但是位置和朝向随机。模板匹配常用的方法有平方差匹配法、相关匹配法、相关系数匹配法、归一化平方差法、归一化相关匹配法模板等等。

在一个实施例中，可以采用一种相关匹配法与归一化相关匹配法相结合，匹配相似度则为两幅图像的相关数值。相关数值越大，则越相似。匹配原理示意图如图7所示。图7从左到右依次图示了将局部地图旋转n度，找出最小有效地图，获取偏移坐标(x_offset_n，y_offset_n)；提取最小有效地图，作为模板；获得最佳匹配位置(local_x_n，local_y_n)。参考图7，一种示例性匹配算法的具体步骤可以包括：1.提取图3中最小有效地图图像，如图8，即将图3进行裁剪后的地图作为模板。2.裁剪后的地图模板(图8)与原局部地图图像(图5)的水平偏移为(x_offset_1，y_offset_1)。3.采用归一化模板匹配算法，找到图4中与图8最佳匹配位置，获得在图4中图8左上角的坐标(local_x_1，local_y_1)，同时获得最佳匹配位置的未归一化相似度result_1。4.将图5顺时针旋转一度，重复前述步骤1、步骤2、步骤3，直到遍历完360度为止。5.获得360个最佳匹配位置，即局部地图遍历360度，每一度均存在一个最佳匹配位置，提取有效地图的水平偏移为(x_offset_n，y_offset_n)，对应的匹配位置的模板的左上角坐标为(local_x_n，local_y_n)和匹配相似度result_n，其中n表示旋转的角度。6.比较360个相似度result_n，找出其最大值(记为result_R)对应的水平偏移(x_offset_R，y_offset_R)、和左上角坐标(local_x_R，local_y_R)。其中则R表明当航向角顺时针旋转R度为最佳匹配角度，从而获得最佳匹配位姿。7.图5中(1024，1024，0)位姿为上电初始位姿，则根据其在图4中的映射关系可以获取移动机器人在图4的全局地图中的初始位姿POSE为：

POSE＝(1024，1024，0)-(x_offset_R，y_offset_R)+(0，0，R)+(local_x_R,local_y_R,0)

＝(1024-x_offset_R+local_x_R，1024-y_)。

根据以上参考图3所描述的方法，可以为移动机器人创建新环境的全局地图，并且可以获取移动机器人上电启动时在全局地图中的绝对位姿，实现移动机器人位姿初始化。可以做到机器人放在任意有效位置点，上电启动时均可确定其准确的初始位姿，即坐标和航向角。

在设置了全局地图并且获取了移动机器人在全局地图中的初始位姿后，便可利用惯性导航定位技术、激光导航定位技术、或者其他适当的导航定位技术或它们的组合对移动机器人在已有全局地图中进行精确定位，实现自动导航功能。

在步骤S120和步骤S240中创建关于移动机器人的位姿发生变化后的周围环境的局部地图的方法，可以采用与步骤S320中创建局部地图的相同的方法。在移动机器人的位姿发生改变并且在步骤S110或者S240中、利用惯性导航传感器计算移动机器人在全局地图中的第一位姿之后，可以设置移动机器人的假定位姿为该第一位姿，开启激光SLAM建图功能，使得移动机器人在原位置自行旋转一周，形成较大的、详细的、360度的局部地图。应当理解，要创建360度的局部地图，需要的移动机器人旋转的角度和移动机器人上布置的激光扫描仪数量以及激光扫描仪视野范围有关。

在步骤S130和步骤S250中将局部地图在全局地图中进行图像匹配，获取移动机器人在全局地图中的第二位姿、和与第二位姿对应的匹配可信度的方法，可以采用与步骤S330中进行局部地图和全局地图匹配的相类似的方法，从而可以获得在激光SLAM技术下的关于惯性导航传感器计算的第一位姿的第二位姿表示、以及匹配可信度(或称为匹配相似度)。

在用于进行图像匹配的一个实施例中，依据当前惯性导航测量元件定义最大误差范围，在根据惯性导航测量的第一位姿(x1，y1，θ1)最大误差范围内，可以用当前激光扫描的局部地图数据与全局栅格地图基于蒙特卡洛方法进行图像匹配，即采用粒子滤波获取最佳位姿、即第二位姿为(x2，y2，θ2)。附加地，还可以计算最佳匹配的匹配可信度。

关于匹配可信度的计算，在获取最佳位姿后，移动机器人可以获得激光扫描范围(在一定角度范围内按一定频率进行扫描)内的激光扫描距离信息，并且将获得的激光扫描距离信息在原有全局栅格地图上进行映射来计算匹配可信度。

移动机器人在导航时有两个坐标系，一是机器人坐标系(即以激光器为坐标原点，激光器前后方向为Y轴，左右方向为X轴)；二是已建立好的全局地图坐标系，即所谓的世界坐标系。全局地图表现的是实际所在环境所有障碍位置的表示，假设在激光观察点在全局地图中(x，y)坐标位置，并且航向角为θ。则从此处向θ角度观察可以获取扫描范围内的障碍物距离信息D1(直接可以从全局地图中获得)。移动机器人配备的激光雷达可以在现实环境中获得扫描范围内的障碍物距离信息D2。D1和D2两个扫描距离信息越相近，则便是可信度越高。这便是激光扫描距离信息与原始地图结合可获得可信度的原理。

实际操作时如下，利用激光雷达在现实中获取的距离信息，映射到全局地图中表示实际障碍所在的坐标，然后与原来全局地图中障碍物做比较，具体做法如下：

在匹配可信度计算时，例如通过惯性导航方式可以获得当前位置和姿态(xc，yc，θc)，其并不精确，表明当前准确的位姿就在其附近。假设获得当前的位姿(xc，yc，θc)，在此位姿激光雷达可扫描出N个距离(障碍)信息，转换成直角坐标且在机器人坐标系(激光器为坐标原点(0，0)，航向角为0)中可以看到激光扫描出的障碍轮廓。而机器人实际在全局地图中的位姿为(xd，yd，θd)，依据映射原理可以算出激光扫描出的障碍轮廓在全局地图中的坐标，即N个坐标。N是激光雷达在一定角度范围内按一定频率进行扫描(即在激光扫描范围内进行扫描)获得的对于障碍物在地图中的像素数量。

在一个实施例中，匹配可信度可以根据激光扫描获得的障碍轮廓在全局地图中的映射轮廓(轮廓形状匹配)的坐标的像素和并且归一化来计算。匹配可信度可以选择全局地图中这N个坐标的像素和并归一化。归一化原因是确保每次获得的可信度范围为一定值，由于激光扫描距离范围限制，假设激光扫描可以扫描点数为180个，但由于距离限制实际上测量到有效距离信息的个数小于或等于180个。因此所有有效扫描点像素和不能直接作为可信度，必须除以有效点个数，则可保证可信度dis_temp一定在0～255间。

进一步可选地，由于在二维栅格地图中，障碍像素值为0，故dis_temp越小表明第二位姿越精确。为适应匹配可信度意义，匹配可信度reliability可以为：reliability＝255-dis_temp。故匹配可信度越大，表明位姿越精确。激光导航测量原理结构图如图9所示。

在一个实施例中，移动机器人可包括2个激光雷达作为激光扫描仪，用于创建全局地图和360度局部地图。图10和图11图示移动机器人所包括的激光扫描仪的可能的布置的示意图。如图10所示，2个激光雷达分别安装在移动机器人的前方和后方，从而可直接观测正前方180度和正后方180度范围的距离信息。从而，在创建360局部地图时，无需移动机器人进行360度的旋转，只需旋转90度。如图11所示，2个激光雷达选用可视角为270度的激光雷达，分别被安装在移动机器人的前方左角和后方右角，分别可以观测正前方和左侧、以及正后方和右侧。从而，可直接观测机器人周围360度范围距离信息，在创建360局部地图时，无需移动机器人进行旋转。

在一个实施例中，可以使用激光扫描仪和电子指南针来创建全局地图和局部地图。建图时可将电子指南针的航向角θ₀作为移动机器人的假定航向角、或者第二位姿包括的航向角。由于每次建图都是以电子指南针航向角为移动机器人的假定航向角，但由于电子指南针自身误差，同时不同位置处可能受外界磁场变化也会引起一定误差，故在创建局部地图时电子指南针的航向角不能直接作为局部地图中的假定航向角，但一定在电子指南针航向角附近处。设电子指南针最大误差为正负15度，则匹配时仅需考虑30度范围内航向角的地图匹配，大大提高匹配速度和准确率。将局部地图与全局地图采用图像处理方案进行图像匹配，得到当前在原始地图中的最佳位姿。由于全局地图和局部地图的创建，都依靠电子指南针的航向角确定移动机器人的假定航向角，从而，在采用诸如SIFT和模板匹配两种算法进行地图匹配时，航向角可以约束在正负15度范围内进行匹配，因此可以极大地降低局部地图和全部地图匹配的复杂度。

参考图12，其图示了根据本发明的一个实施方式的移动机器人位姿确定方法1200的流程图。方法1200在步骤S260之前的步骤与方法200的步骤相同。方法1200进一步包括步骤S270-S278，用于根据匹配可信度，确定移动机器人的位姿应当为惯性导航计算的第一位姿，还是激光导航计算的第二位姿。

在图12所示的实施方式中，匹配可信度越大，表明位姿越精确。所示意的激光导航和惯性导航选择策略为：当激光导航策略中匹配可信度高于一定阈值时，表明激光导航确定的第二位姿有效，移动机器人的位姿为第二位姿；否则激光导航确定的第一位姿无效，移动机器人的位姿为惯性导航确定的第一位姿。如此，可消除由于外界环境临时变动而导致激光导航策略获得的错误绝对位姿。

在图12所示的实施方式中还能够实现惯性导航的累计误差消除，其原理可以为：通过每时刻激光导航策略均对惯性导航策略的位姿进行修正，故避免了惯性导航累计误差的出现；然而，当环境临时变动时，可导致当前位姿一定时间内连续使用惯性导航数据，即使环境恢复，由于累计误差原因，使得当前位姿与实际位姿存在误差，可导致激光导航策略中匹配可信度永远不满足阈值，从而仅有惯性导航。故当检测到激光导航可信度连续不满足阈值一定时，需要重新进行初始位姿定位，以此循环。

在进一步的实施方式中，可以进一步确定移动机器人惯性导航技术计算的第一位姿的位姿误差，并且对该位姿误差进行误差校正。下面结合图13-图19来描述具体的误差校正方法。

图13图示了现有技术的室内定位和导航技术的方法1300的流程图。

步骤S1310，室内地图创建。室内地图的创建可以采用卷尺和激光测距仪等方法，测量墙壁的长度和厚度以及相对坐标原点的位置，基于直角坐标系建立室内地图数据库。

步骤S1320，路径规划。路径规划一般是指机器人在有障碍物的环境中，按照一定的评价标准，寻找一条从当前位置到目标位置的无碰撞路径。移动机器人导航引擎在得到目标位置与当前位置信息后，就需要根据地图，计算出最优的路径、以及可能的备选路径，将移动机器人导航到目的地。常见的路径规划算法例如包括A*和Dijkstra算法。

步骤S1330，室内导航。移动机器人导航引擎使用所规划的路径将移动机器人导航到目标位置。

移动机器人导航引擎可以采用的定位和导航技术例如可以是基于超宽带(UWB)的定位和导航技术、基于惯性的定位和导航技术、基于无线局域网(WLAN)的定位和导航技术、激光测距定位和导航技术、基于视觉导航的定位和导航技术、基于超声波测距的定位和导航技术、或者它们中的一项或多项的结合。本领域技术人员应当能够理解这些定位和导航技术的基本原理、和位姿误差产生的原因，然而考虑到本文的完整性，下面仅以超宽带(UWB)技术和惯性定位和导航技术为例做个简单说明。

超宽带可以测量基站和标签之间的距离信息。将标签安装到移动机器人上，当标签同时测量到三个及以上基站的距离信息时，采用三边测量法就可以计算出移动机器人的位置和航向。

图14图示了基于超宽带(UWB)的定位和导航技术的示意图。如图14所示，具体计算步骤可以如下：

1)基站A、B、C的位置为已知，分别为(x_a,y_a)、(x_b,y_b)、(x_c,y_c)，利用基站发射的超宽带信号测量的其距离标签T的距离分别为r_a、r_b和r_c；

2)标签T的位置为(x_t,y_t)。则根据步骤1)的信息，可得出以下公式：

${(x_t-x_a)}^2+{(y_t-y_a)}^2={r_a}^2$

${(x_t-x_b)}^2+{(y_t-y_b)}^2={r_b}^2$

${(x_t-x_c)}^2+{(y_t-y_c)}^2={r_c}^2$

3)由步骤2)中公式，可计算出标签T的位置(x_t,y_t)为：

$x_t=\frac{(r_a^2-x_a^2-y_a^2)\·(y_c-y_b)+(r_b^2-x_b^2-y_b^2)\·(y_a-y_c)+(r_c^2-x_c^2-y_c^2)\·(y_b-y_a)}{2\[(x_b-x_a)\·(y_c-y_a)-(x_c-x_a)\·(y_b-y_a)\]}$

$y_t=\frac{(r_a^2-x_a^2-y_a^2)-(r_b^2-x_b^2-y_b^2)-2\·x_t\·(x_b-x_a)}{2\·(y_b-y_a)}$

4)根据当前时刻标签T的位置(x_t,y_t)和上一时刻标签T的位置(x_t0,y_t0)，可计算标签T的航向Ψ为：

$\mathrm{\ψ}=arctan\left(\frac{y_t-y_{t0}}{x_t-x_{t0}}\right)$

可见，在超宽带室内位姿中，引起位姿误差的因素有两个，一是UWB信号的传播效应，包括非视距传播效应、多径传播效应及远近效应等，二是由于当前时刻标签位置的计算依赖于前一时刻标签位置所带来的累积误差。

惯性定位和导航技术是通过陀螺仪、加速计、里程计等传感器来进行移动机器人的定位和导航。陀螺仪测量移动机器人的角速度，里程计测量移动机器人的移动距离，陀螺仪/里程计可以计算移动机器人的相对变化位置。具体步骤如下：

1)在室内布置多个超宽带基站，并记录基站在室内地图下的位置信息；

2)当组合导航未初始化位置和航向时，如果标签测量到的基站距离信息小于三个，则移动机器人自由行走，直到测量到的基站距离信息不少于三个；

3)当标签测量到三个及以上基站数据时，移动机器人根据标签和基站的距离信息计算移动机器人的位置和航向，完成组合导航的位置和航向初始化；

4)移动机器人根据当前时刻的位置和航向，基于陀螺仪的角速度数据和里程计的距离数据计算下一时刻的位置和航向；

5)当标签测量到三个及以上基站数据时，计算出移动机器人的位置和航向，并与步骤4)所得位置和航向按照设置的比例参数进行融合，并作为移动机器人的真实位置和航向；

6)当标签未测量到三个基站数据时，步骤4)所得位置和航向即为移动机器人的真实位置和航向。

可见，惯性定位和导航技术所使用的传感器的精度会产生即时误差，依赖于前一时刻位姿来计算当前时刻位姿会带来累积误差。

参考图15，其图示根据本发明的实施方式的用于移动机器人位姿的方法300的流程图。方法1500可以用于确定移动机器人室内位姿方法的位姿误差，并且可选地，可以进行位姿误差校正。位姿可以包括移动机器人的位置和航向，位姿误差可以包括位置误差和航向误差。应当理解，方法1500可以结合在方法1300的描述所提及的任何一种定位和导航技术而使用。

步骤S1510，根据移动机器人室内定位技术估计移动机器人的当前位姿。

步骤S1520，访问参考物数据库，所述参考物数据库包括移动机器人配备的摄像头拍摄的、关于至少一个参考物中的每个参考物的一幅或多幅参考图像，以及与所述参考图像关联的、所述摄像头拍摄参考图像时移动机器人的真实位姿，其中所述参考物是用来在其中位姿移动机器人的环境中的静止物体，并且从所述参考物数据库中选择一个真实位姿和相关联的参考图像。

在一个实施例中，可以从参考物数据库中选择与所选择的当前位姿所需距离最短的真实位姿。在一个实施例中，可以从参考物数据库中选择从当前位姿导航到所需时间最短的真实位姿。在一个实施例中，可以选择在所需距离和所需时间两个方面较为折中的真实位姿。

步骤S1530，将移动机器人导航至所选择的真实位姿，即目标位姿，所述目标位姿是根据所述移动机器人室内定位技术所估计出的。

步骤S1540，利用移动机器人配备的摄像头进行图像拍摄，获得拍摄图像。应当注意，该拍摄图像中需要包括参考物图像，该参考物也出现在所选择的相关联的参考图像中；否则，表明移动机器人室内定位技术的累积误差过大了，使得参考物不能出现在移动机器人配备的摄像头的视野中了，至少应当在累积误差积累到出现这种情况之前进行误差校正。在一个实施例中，可以据出现这种情况来定义误差校正阈值。

步骤S1550，将所述拍摄图像中包括的参考物图像(也称为实际参考物图像)与所选择的相关联的参考图像中包括的参考物图像(也称为数据库参考物图像)进行对比。对比可以包括形状和/或尺寸方面的对比。形状对比是指判断实际参考物图像相对于数据库参考物图像是否存在变形，比如向左倾斜、向右倾斜，这是由于拍摄时的摄像头的方向不同(表明机器人的航向不同)所引起的。尺寸对比是指判断实际参考物图像相对于数据库参考物图像在形状相同的前提下是否存在大小变化，比如尺寸偏小、尺寸偏大，这是由于拍摄时的摄像头距离参考物的距离不同所引起的。

步骤S1560，根据对比的结果来确定移动机器人室内定位技术产生的位姿误差。

在可选的步骤S1570，必要时，可以利用所确定的位姿误差对步骤S1510中所估计的移动机器人的当前位姿进行误差校正。可以只在确定的位姿误差超过预设的阈值时，才执行此校正步骤。例如，该阈值可以是进行了预设次数的定位和导航，比如执行了3次惯性导航后执行一次位姿误差校正。

进一步地，在步骤S1520之前，方法1500还可以包括步骤S1515，确定是否需要对步骤S1510中所估计的移动机器人的当前位姿进行误差确定(未示出)。只有在估计当前位姿的误差可能超过误差阈值时，才执行步骤S1520及后续进行，进行位姿误差确认。例如，该误差阈值可以是进行了预设次数的定位和导航，比如执行了3次惯性导航后执行一次位姿误差确定，还可以是航向误差超过了预设的航向误差阈值、和/或位置误差超过了预设的位置误差阈值；或者，该误差阈值可以依据所采用移动机器人室内定位技术而定；或者，该误差阈值可以依据定位所使用传感器的精度而定。

可选地，方法1500还可以包括建立参考物数据库的步骤。应当理解，无需针对每次误差确定或者误差校正的方法1500的流程都执行建立参考物数据库的步骤。预先建立的参考物数据库也可以为家庭内的不同的移动机器人所共用。可以将参考物数据库存储在移动机器人中，也可以将参考物数据库存储在移动机器人之外的服务器中，在步骤S1520中，移动机器人需要与该服务器通信来获取从所述参考物数据库中选择的真实位姿和相关联的参考图像。

参考图16，其图示了根据本发明的实施方式的用于移动机器人定位和导航的方法400的流程图。方法400可以理解为是将图14所示的位姿误差确定和校正方法应用于图13所示的移动机器人定位和导航方法的一个具体实例。

步骤S402，室内地图创建。

步骤S404，建立参考物数据库，具体建立步骤可以如下进行：

1)可以选择固定在墙壁上的电灯开关、消防警钟、电视等作为参考物。所选参考物不局限于以上物体，所有固定在墙壁上的易识别物体和标记均在所述范围之内；或者，还例如可以是固定摆放的家具(比如电视、饮水机等)、或者家具上的标记(比如Logo、电子家具的开关按键)等。

2)将移动机器人调整到处于参考物前方，开启摄像头拍摄参考物，存储参考物图像至参考物数据库，并用卷尺或激光测距仪等准确测量此时(进行拍摄时)移动机器人的位姿，包括位置P_true和航向A_true，建立参考物图像与拍摄时的机器人位姿P_true和航向A_true一一对应的参考物数据库记录。参考物数据库不局限某一个参考物，可建立多个参考物图像与拍摄位置和航向的数据库，且同一幅图像中可同时包含多个参考物。

3)以多个角度拍摄同一参考物，和/或以一个或多个角度拍摄不同的参考物，按照步骤2)建立参考物数据库。

步骤S410，根据移动机器人室内定位技术估计移动机器人的当前位姿。

步骤S420，访问参考物数据库，并且从所述参考物数据库中选择一个真实位姿和相关联的参考图像。

步骤S422，路径规划。比如，当监测到定位传感器出现故障、定位统受到环境干扰等情况时，这可能会导致定位或定向误差超过阈值，移动机器人需要利用参考物进行定位和定向误差确定和修正。移动机器人可以根据当前位姿从参考物数据库中选择距离最近的参考物图像，并将所对应的拍摄位置和航向作为目标定向，根据室内地图计算出从当前位置到目标点的最佳路径，避开墙壁等各种障碍物，指导移动机器人按照规划路径行驶。

步骤S424，室内导航。移动机器人导航引擎使用步骤S422中所计算的路径将移动机器人导航到目标定向。

步骤S430，将移动机器人导航至所选择的真实位姿，即目标位姿，所述目标位姿是根据所述移动机器人室内定位技术所估计出的。

步骤S440，利用移动机器人配备的摄像头进行图像拍摄，获得拍摄图像。

移动机器人到达目的定向后，可以打开摄像头进行数据采集，将摄像画面作为图像存储。由于室内导航存在误差，移动机器人的位置和航向与预期存在差距，导致拍摄的实际参考物与数据库参考物相比，可能存在变形、尺寸偏小和尺寸偏大等问题。

步骤S450，将所述拍摄图像中包括的参考物图像(也称为实际参考物图像)与所选择的相关联的参考图像中包括的参考物图像(也称为数据库参考物图像)进行对比。图17A、图17B和图17C图示摄像头拍摄的实际参考物图像相对于数据库参考物图像所可能发生的变化的示意图，分别图示了实际参考物图像发生变形、尺寸偏大、尺寸偏小的情形。

步骤S460，根据对比的结果来确定移动机器人室内定位技术产生的位姿误差，具体可以包括步骤S4602和S4604。

在步骤S4602，根据对比的结果调整所述移动机器人的航向，使得移动机器人配备的摄像头所拍摄的图像中的参考物图像与所选择的相关联的参考图像中包括的参考物图像趋于一致。

根据摄像头采集的实际参考物图像和数据库参考物图像进行对比，分析两者的匹配情况，判断实际参考物的状态，包括变形、尺寸偏小和尺寸偏大三种。当移动机器人位置偏左、偏右或拍摄航向与数据库的移动人定向不一致时，会导致参考物变形；当移动机器人位置偏前或偏后时，会导致参考物尺寸偏大或偏小。

可以通过调整移动机器人的位置和航向，校正参考物的拍摄效果，实现参考物的拍摄效果与数据库参考物的某一个图像一致。

在步骤S4604，确定所述移动机器人所调整的位姿变化，即为所述移动机器人室内定位技术产生的当前位姿误差，包括位置误差P_err和航向误差A_err。

步骤S470，利用所确定的位姿误差对步骤S410中所估计的移动机器人的当前位姿进行误差校正。在步骤S460中计算出位置误差P_err和航向误差A_err后，可以对步骤S410中所估计的位姿，即位置P1和航向A1，进行校正，得到移动机器人的校正后的位姿，位置P₁'和A₁'：

P₁'＝P₁-P_err

A₁'＝A₁-A_err

在步骤S1560和步骤S460中，可以有多种方式确定移动机器人室内定位技术产生的位姿误差。

在第一实施方式中，可以只采用参考物数据库中的参考物的一幅图像，通过调整移动机器人的位置和航向，使拍摄的实际参考物与数据库参考物达到最佳匹配度来进行误差确定，可以执行如下步骤。

步骤S1a，设置移动机器人导航引擎使得移动机器人左右移动的最小距离为d，左右旋转最小角度为θ，预先设定在进行位姿误差确定时要进行的移动次数为左右各为m，要进行的旋转次数左右各为n。移动机器人未进行调整前的位置为P₀，航向为A₀。

步骤S1b，在移动机器人的当前位姿(P₀，A₀)，保持移动机器人位置不动，将航向依次向左旋转0*θ、1*θ、…、n*θ拍摄参考物，并相应计算实际参考物图像与数据库参考物图像的匹配度S₀、S₁、…、S_n。然后，向右旋转n*θ，回到航向A₀。接着，将机器人的航向依次向右旋转1*θ、2*θ、…、n*θ拍摄参考物，并相应计算实际参考物图像与数据库参考物图像的匹配度S_n+1、S_n+2、…、S_2n。然后，向左旋转n*θ，回到航向A₀。则，该位姿(P₀，A₀)所拍摄的实际参考物图像与数据库参考物图像的最高匹配度为D₀＝MAX{S₀，S₁，…，S_2n}，记录该位置的最高匹配度D₀、和与D₀对应的左右旋转的航向Y₀(Y₀＝j*θ，j属于{-n,…,n}中的某个值)，其中j表示取得最高匹配度D₀的航向左右旋转的次数。还同时记录移动机器人左右移动的距离L₀(L₀＝i*d,i属于{-m,…,m}中的某个值)。其中i表示移动机器人相对于位姿(P₀，A₀)进行左右移动的次数，此时，由于移动机器人未进行左右移动，所以i＝0。

步骤S1c，移动机器人依次向左移动1*d、2*d、…、m*d，并在每个位置执行步骤S1b，计算并记录相应的最高匹配度D_i以及对应的左右移动距离和航向，从而，有{D₁，L₁，Y₁}，{D₂，L₂，Y₂}，…，{D_m，L_m，Y_m}。

步骤S1d，移动机器人回到当前位姿(P₀，A₀)。然后，移动机器人依次向右移动1*d、2*d、…、m*d，并在每个位置执行步骤S1b，计算并记录相应的最高匹配度D_i以及对应的左右移动距离和航向，从而，有{D_-1，L_-1，Y_-1}，{D_-2，L_-2，Y_-2}，…，{D_-m，L_-m，Y_-m}。

步骤S1e，在不考虑尺寸问题的情况下，实际参考物图像与数据库参考物图像的最佳匹配度为D＝MAX{D_-m，…，D_-1，D₀，D₁，…，D_m}。根据D所对应的左右移动距离L和左右旋转航向Y调整移动机器人，该状态下所拍摄的实际参考物图像与数据库参考物图像只存在尺寸不一致问题，不存在变形。

步骤S1f，若此时实际参考物图像与数据库参考物图像尺寸也相等，则参考物图像的特征匹配完成，如图18所示，其示出了实际参考物图像与数据库参考物图像尺寸刚好匹配的情形。记录移动机器人此时的位姿为(P₁，A₁)，为室内位姿系统的位置和航向输出。则此时，位置误差P_err＝P₁-P_true，航向误差A_err＝A₁-A_true，P_true和A_true为与数据库参考物图像对应的参考物数据库中的移动机器人位姿，包括位置和航向。

若此时实际参考物图像比数据库参考物图像尺寸小，则调整移动机器人前进，直到两者尺寸相等，参考物特征匹配完成，如图18所示。记录移动机器人此时的位姿为(P₁，A₁)，为室内位姿系统的位置和航向输出。则此时，位置误差P_err＝P₁-P_true，航向误差A_err＝A₁-A_true，P_true和A_true为与数据库参考物图像对应的参考物数据库中的移动机器人位姿，包括位置和航向。

若此时实际参考物图像比数据库参考物图像尺寸大，则调整移动机器人后退，直到两者尺寸相等，参考物特征匹配完成，如图18所示。记录移动机器人此时的位姿为(P₁，A₁)，为室内位姿系统的位置和航向输出。则此时，位置误差P_err＝P₁-P_true，航向误差A_err＝A₁-A_true，P_true和A_true为与数据库参考物图像对应的参考物数据库中的移动机器人位姿，包括位置和航向。

在第一实施方式中，需多次调整移动机器人，比较耗时，且对移动机器人进行左右移动的调整比较麻烦。为此，在第二实施方式中，可采用数据库中一个参考物的多幅图像进行特征匹配，避免左右移动。

在第二实施方式中，参考物数据库包括一个参考物的多幅图像，该多幅图像包括移动机器人在一个拍摄位置拍摄的一幅图像、和在该位置左右各移动m次拍摄的2m幅图像。机器人每次能够左右移动的距离是确定的，例如为d。在第二实施方式中，采用数据库的多幅参考物图像进行匹配，通过调整移动机器人左右旋转(调整航向)和前后移动实现参考物特征匹配，使拍摄的实际参考物与数据库参考物达到最佳匹配度来进行误差确定，可以执行如下步骤：

步骤S2a，保持移动机器人位置不动，调整移动机器人左右旋转(依次改变航向角)，使实际参考物图像与多幅数据库参考物图像中的某幅图像的匹配度最高，该状态下所拍摄的实际参考物图像与数据库参考物图像只存在尺寸不一致问题，不存在变形。

步骤S2b，当实际参考物图像与数据库参考物图像尺寸相等时，参考物特征匹配完成；当实际参考物图像尺寸比数据库参考物图像尺寸小时，调整移动机器人前进，直到两者尺寸相等，参考物特征匹配完成；当实际参考物图像尺寸比数据库参考物图像尺寸大时，调整移动机器人后退，直到两者尺寸相等，参考物特征匹配完成。在后两种情况下，将移动机器人移动到参考物数据库中的与最匹配的数据库参考物关联的移动机器人拍摄该图像时的位置。记录移动机器人最后的位姿为(P₁，A₁)，为室内位姿系统的位置和航向输出。则此时，位置误差P_err＝P₁-P_true，航向误差A_err＝A₁-A_true，P_true和A_true为与数据库参考物图像对应的参考物数据库中的移动机器人位姿，包括位置和航向。

在第三实施方式中，由于室内地面通常是平坦的，可以假设移动机器人摄像头的高度是固定不变的，则参考物在摄像画面中的相对高度是不变的，只存在左右偏差。图19图示了移动机器人拍摄画面(也称为摄影画面)中的数据库参考物图像和实际参考物图像的可能的偏差。因此，有可能通过偏差距离来反推航向误差，避免移动机器人旋转，减少调整过程，具体步骤如下：

步骤S3a，数据库参考物图像和实际参考物图像距离摄影画面左边缘的距离分别为d1和d2。所述距离可以不局限于左边缘距离，还可以是右边缘距离、或者左边缘和右边缘的结合。

步骤S3b，假设移动机器人旋转角度与摄影画面中的距离系数为f，系数f可以用移动机器人拍摄数据库参考物图像时距离参考物的距离和航向来表示，可以将其与数据库参考物图像相关联的存储在参考物数据库中，则移动机器人的航向误差为：

$A_{err}=\frac{d2-d1}{f}$

经过航向误差A_err校正后所拍摄的实际参考物图像与数据库参考物图像只存在尺寸不一致问题，不存在变形。

步骤S3c，当实际参考物图像与数据库参考物图像尺寸相等时，参考物特征匹配完成；当实际参考物图像尺寸比数据库参考物图像尺寸小时，调整移动机器人前进，直到两者尺寸相等，参考物特征匹配完成；当实际参考物图像尺寸比数据库参考物图像尺寸大时，调整移动机器人后退，直到两者尺寸相等，参考物特征匹配完成。位置误差P_err可以采用第一或者第二实施方式的确定方式。

在第三实施方式中，可以通过实际参考物图像和数据库参考物图像距离摄影画面边缘的距离差来计算航向误差，移动机器人无需左右旋转，但仍需前后调整来计算位置误差。

在第四实施方式中，由于参考物的尺寸与摄像头和参考物的距离有关，因此可根据参考物尺寸差来计算位置误差，具体步骤如下：

步骤S4a，数据库参考物图像和实际参考物图像距离摄影画面左边缘的距离分别为d1和d2。所述距离可以不局限于左边缘距离，还可以是右边缘距离、或者左边缘和右边缘的结合。

步骤S4b，假设移动机器人旋转角度与摄影画面中的距离系数为f，则移动机器人的航向误差为：

$A_{err}=\frac{d2-d1}{f}$

经过航向误差A_err校正后所拍摄的实际参考物图像与数据库参考物图像只存在尺寸不一致问题，不存在变形。

步骤S4c，参考物数据库中参考物尺寸(比如高度)为h1，实际参考物尺寸(比如高度)为h2，摄像头与参考物的距离和尺寸的系数为v，系数v可以用移动机器人拍摄数据库参考物图像时距离参考物的距离来表示，可以将其与数据库参考物图像相关联的存储在参考物数据库中，则移动机器人的位置误差为：

$P_{err}=\frac{h2-h1}{v}$

应当理解，附图中所示流程图图示了按照本发明各种实施例的方法可能实现的功能和操作，在有些作为替换的实现中，方法流程的执行顺序可以以不同于附图中所标注的顺序发生。

在另一个方面，应当理解，本发明实施方式所提出的各种方法可以被实现为软件、硬件或者软件和软件的组合。硬件可通过被设计来执行上述功能的转用集成电路(ASIC)、数字信号处理器(DSP)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微处理器、其他电子单元或其组合来实现。软件可通过执行上述功能的模块来实现。软件可被存储在存储单元中并由处理器运行。作为存储单元或处理器，可采用本领域技术人员公知的各种单元。因此，本公开内容也意图涵盖能够实现所有这些方法的功能模块架构的装置，这些装置可以被依软件、硬件、或者软件和硬件的结合来实现。

举例而言，本发明实施方式提供了一种移动机器人的位姿确定设备，所述移动机器人包括激光扫描仪和惯性导航传感器。所述设备可以包括：第一位姿确定装置，用于在所述移动机器人的位姿发生改变时，利用所述惯性导航传感器计算所述移动机器人在全局地图中的第一位姿；局部地图创建装置，用于使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于所述移动机器人的周围环境的局部地图；第二位姿确定装置，用于将所述局部地图在全局地图中进行图像匹配，获取所述移动机器人在所述全局地图中的第二位姿、和与所述第二位姿对应的匹配可信度，其中所述第二位姿为图形匹配得出的所述移动机器人在所述全局地图中的最佳位姿，并且其中所述全局地图是使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建的关于所述移动机器人在其中导航的环境的地图，以及最终位姿确定装置，用于根据所述匹配可信度，确定所述移动机器人的位姿为所述第一位姿或者所述第二位姿。

在一个实施方式中，所述设备还可以包括：全局地图创建装置，用于在检测到移动机器人出现在新环境中时，使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建所述全局地图，以及位姿初始化装置，用于在移动机器人上电时对移动机器人的位姿进行初始化。

在一个实施方式中，所述最终位姿确定装置可以进一步用于，当所述匹配可信度大于第一阈值时，确定所述移动机器人的位姿为所述第二位姿，并且当所述匹配可信度小于或者等于所述第一阈值时，确定所述移动机器人的位姿为所述第一位姿。

在一个实施方式中，所述设备还可以包括：全局地图重建装置，用于统计所获取的匹配可信度小于或者等于所述第一阈值的次数，其中当所统计的次数大于第二阈值时，使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术重新创建所述全局地图。

在一个实施方式中，所述匹配可信度可以为使用所述激光扫描仪在所述局部地图中进行激光扫描获得的障碍轮廓在所述全局地图中的映射轮廓的坐标的像素和、并且对所述像素和进行归一化的结果。

在一个实施方式中，所述设备还可以包括：统计装置，用于统计所获取的匹配可信度小于或者等于所述第一阈值的次数，其中当所统计的次数大于第二阈值时，所述设备进一步包括：访问装置，用于访问参考物数据库，所述参考物数据库包括所述移动机器人配备的摄像头拍摄的、关于至少一个参考物中的每个参考物的一幅参考图像，以及与所述参考图像关联的、所述摄像头拍摄所述参考图像时所述移动机器人的相应真实位姿，并且从所述参考物数据库中选择一个真实位姿和相关联的参考图像，其中所述参考物是室内环境中的静止物体；导航装置，用于将所述移动机器人导航至目标位姿，所述目标位姿的数值是根据惯性导航定位技术对于所选择的真实位姿的估计值；拍摄装置，用于利用所述移动机器人配备的所述摄像头进行图像拍摄，获得拍摄图像；对比装置，用于将所述拍摄图像中包括的参考物图像、即实际参考物图像与所选择的相关联的参考图像中包括的参考物图像、即数据库参考物图像进行对比；确定装置，用于根据对比的结果来确定所述惯性导航传感器计算的第一位姿的位姿误差；以及误差确定装置，用于利用所确定的位姿误差对所述第一位姿进行误差校正。

在一个实施方式中，所述确定装置可以进一步包括：调整和特征匹配装置，用于根据对比的结果调整所述移动机器人的位姿，并且对所述移动机器人配备的所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像进行特征匹配，使得达到最佳匹配度；以及位姿误差确定装置，用于根据特征匹配的结果来确定所述第一位姿的位姿误差。

在一个实施方式中，所述调整和特征匹配装置可以进一步包括：第一调整和特征匹配装置，用于通过对移动机器人进行以下各项操作中的一项或多项：向左和/或向右移动、向左和/或向右旋转、以及向前和/或向后调整，使得所述移动机器人配备的所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在形状方面和尺寸方面达到最佳匹配度；第二调整和特征匹配装置，用于通过向左和/或向右旋转移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在形状方面达到最佳匹配度；或者第三调整和特征匹配装置，用于通过向前和/或向后调整移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在尺寸方面达到最佳匹配度。

在一个实施方式中，所述参考物数据库还可以包括与所述移动机器人的一个真实位姿相关联的多幅参考图像，所述多幅参考图像包括所述移动机器人配备的摄像头在所述移动机器人的如下位姿拍摄的关于参考物的参考图像集合、以及拍摄每幅参考图像时的所述移动机器人的相应真实位姿：所述移动机器人的所述真实位姿，在所述移动机器人的真实位姿处向左和向右各旋转n次所述移动机器人的最小旋转角度，在所述移动机器人的真实位姿处向左和向右各移动m次所述移动机器人的最小移动距离，以及在所述移动机器人的真实位姿处向左和向右各移动m次所述移动机器人的最小移动距离并且在每个移动后的位置处向左和向右各旋转n次所述移动机器人的最小旋转角度，其中m和n为自然数。所述调整和特征匹配装置可以进一步包括：第四调整和特征匹配装置，用于通过向左和/或向右旋转移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述多幅参考图像中包括的数据库参考物图像在形状方面达到最佳匹配度；以及第五调整和特征匹配装置，用于通过向前和/或向后调整移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在尺寸方面达到最佳匹配度。

在一个实施方式中，所述确定装置可以包括：第一确定装置，用于根据所述惯性导航定位技术计算特征匹配后的所述移动机器人的位姿，并且根据所述特征匹配后的位姿和所述参考物数据库中存储的、与拍摄所述达到最佳匹配度的参考图像时的所述移动机器人的相应真实位姿来确定所述第一位姿的位姿误差。

在一个实施方式中，所述确定装置可以进一步包括：第二确定装置，用于通过所述实际参考物图像和所述数据库参考物图像距离各自拍摄图像左边缘或者右边缘的距离差来确定航向误差；或者第三确定装置，用于通过移动机器人拍摄所述实际参考物图像时距离参考物的距离、和参考物数据库中存储的移动机器人拍摄数据库参考物图像时距离参考物的距离来确位姿误差。

本发明实施方式的教导还可以实现为一种可以在计算机可读存储介质中实现的计算机程序产品，所述计算机可读存储介质具有存储于其中的计算机可读程序代码部分，所述计算机可读程序代码部分被配置为执行如前所述的根据本发明实施方式的方法或者所述方法的各个步骤。

根据本发明的实施方式，提供了基于激光扫描仪和惯性导航传感器的组合导航方法，在已有全局地图中激光导航可以修正惯性导航带来的累计误差，惯性导航可以修正外界环境临时变动造成激光导航策略定位错误。根据本发明实施方式，当机器人处于新环境中时可以自动创建全局地图，从而无需对环境进行任何改造，也无需提供室内平面图，适用性强，灵活性高，紧靠机器人自身传感器可以完成自主导航。

根据本发明的实施方式，能够客服单个室内定位导航策略各自存在的缺点，能够同时满足当前家庭服务机器人的如下主流导航需求，其主要包括：1.导航路线自由灵活；2.环境改造尽可能小；3.室内定位准确度高；4.不同环境下适应性尽可能高，避免提前坐标标定；5.定位长期稳定、可靠、鲁棒性高。进一步可选地，根据本发明的实施方式，可以及时发现移动机器人在室内导航过程中所产生的位姿误差，并且进一步地，在必要时可以对该位姿误差进行校正。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种移动机器人的位姿确定方法，所述移动机器人包括激光扫描仪和惯性导航传感器，其特征在于，所述方法包括：

在所述移动机器人的位姿发生改变时，利用所述惯性导航传感器计算所述移动机器人在全局地图中的第一位姿；

使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于所述移动机器人的周围环境的局部地图；

将所述局部地图在全局地图中进行图像匹配，获取所述移动机器人在所述全局地图中的第二位姿、和与所述第二位姿对应的匹配可信度，其中所述第二位姿为图形匹配得出的所述移动机器人在所述全局地图中的最佳位姿，并且其中，所述全局地图是使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建的关于所述移动机器人在其中导航的环境的地图；以及

根据所述匹配可信度，确定所述移动机器人的位姿为所述第一位姿或者所述第二位姿。

2.根据权利要求1所述的方法，其特征在于，还包括：

在检测到移动机器人出现在新环境中时，使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建所述全局地图，以及

在移动机器人上电时对移动机器人的位姿进行初始化。

3.根据权利要求1所述的方法，其特征在于，所述根据所述匹配可信度，确定所述移动机器人的位姿为所述第一位姿或者所述第二位姿进一步包括：

当所述匹配可信度大于第一阈值时，确定所述移动机器人的位姿为所述第二位姿，并且当所述匹配可信度小于或者等于所述第一阈值时，确定所述移动机器人的位姿为所述第一位姿。

4.根据权利要求1-3中任一项所述的方法，其特征在于，所述匹配可信度为使用所述激光扫描仪在所述局部地图中进行激光扫描获得的障碍轮廓在所述全局地图中的映射轮廓的坐标的像素和、并且对所述像素和进行归一化的结果。

5.根据权利要求3所述的方法，其特征在于，还包括：

统计所获取的匹配可信度小于或者等于所述第一阈值的次数，其中当所统计的次数大于第二阈值时，所述方法进一步包括：

访问参考物数据库，所述参考物数据库包括所述移动机器人配备的摄像头拍摄的、关于至少一个参考物中的每个参考物的一幅参考图像，以及与所述参考图像关联的、所述摄像头拍摄所述参考图像时所述移动机器人的相应真实位姿，并且从所述参考物数据库中选择一个真实位姿和相关联的参考图像，其中所述参考物是室内环境中的静止物体；

将所述移动机器人导航至目标位姿，所述目标位姿的数值是根据惯性导航定位技术对于所选择的真实位姿的估计值；

利用所述移动机器人配备的所述摄像头进行图像拍摄，获得拍摄图像；

将所述拍摄图像中包括的参考物图像、即实际参考物图像与所选择的相关联的参考图像中包括的参考物图像、即数据库参考物图像进行对比；

根据对比的结果来确定所述惯性导航传感器计算的第一位姿的位姿误差；以及

利用所确定的位姿误差对所述第一位姿进行误差校正。

6.根据权利要求5所述的方法，其特征在于，根据对比的结果来确定所述第一位姿的位姿误差进一步包括：

根据对比的结果调整所述移动机器人的位姿，并且对所述移动机器人配备的所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像进行特征匹配，使得达到最佳匹配度；以及

根据特征匹配的结果来确定所述第一位姿的位姿误差。

7.根据权利要求6所述的方法，其特征在于，所述根据对比的结果调整所述移动机器人的位姿，并且对所述移动机器人配备的所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像进行特征匹配，使得达到最佳匹配度包括：

通过对移动机器人进行以下各项操作中的一项或多项：向左和/或向右移动、向左和/或向右旋转、以及向前和/或向后调整，使得所述移动机器人配备的所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在形状方面和尺寸方面达到最佳匹配度；

通过向左和/或向右旋转移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在形状方面达到最佳匹配度；或者

通过向前和/或向后调整移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在尺寸方面达到最佳匹配度。

8.根据权利要求6所述的方法，其特征在于，所述参考物数据库还包括与所述移动机器人的一个真实位姿相关联的多幅参考图像，所述多幅参考图像包括所述移动机器人配备的摄像头在所述移动机器人的如下位姿拍摄的关于参考物的参考图像集合、以及拍摄每幅参考图像时的所述移动机器人的相应真实位姿：所述移动机器人的所述真实位姿，在所述移动机器人的真实位姿处向左和向右各旋转n次所述移动机器人的最小旋转角度，在所述移动机器人的真实位姿处向左和向右各移动m次所述移动机器人的最小移动距离，以及在所述移动机器人的真实位姿处向左和向右各移动m次所述移动机器人的最小移动距离并且在每个移动后的位置处向左和向右各旋转n次所述移动机器人的最小旋转角度，其中m和n为自然数，

所述根据对比的结果调整所述移动机器人的位姿，并且对所述移动机器人配备的所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像进行特征匹配，使得达到最佳匹配度包括：

通过向左和/或向右旋转移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述多幅参考图像中包括的数据库参考物图像在形状方面达到最佳匹配度；以及

通过向前和/或向后调整移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在尺寸方面达到最佳匹配度。

9.一种移动机器人的位姿确定设备，所述移动机器人包括激光扫描仪和惯性导航传感器，其特征在于，所述设备包括：

第一位姿确定装置，用于在所述移动机器人的位姿发生改变时，利用所述惯性导航传感器计算所述移动机器人在全局地图中的第一位姿；

局部地图创建装置，用于使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于所述移动机器人的周围环境的局部地图；

第二位姿确定装置，用于将所述局部地图在全局地图中进行图像匹配，获取所述移动机器人在所述全局地图中的第二位姿、和与所述第二位姿对应的匹配可信度，其中所述第二位姿为图形匹配得出的所述移动机器人在所述全局地图中的最佳位姿，并且其中，所述全局地图是使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建的关于所述移动机器人在其中导航的环境的地图；以及

最终位姿确定装置，用于根据所述匹配可信度，确定所述移动机器人的位姿为所述第一位姿或者所述第二位姿。

10.一种在计算机可读存储介质中实现的计算机程序产品，所述计算机可读存储介质具有存储于其中的计算机可读程序代码部分，所述计算机可读程序代码部分被配置为执行根据权利要求1-8所述的方法。