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

移动机器人的位姿确定方法和设备的制造方法
【专利摘要】本发明的实施方式涉及移动机器人的位姿确定方法和设备。该方法包括：使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建移动机器人在其中导航的环境的全局地图；在移动机器人上电时，使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建针对所述移动机器人的上电时刻的环境的局部地图；以及将局部地图在述全局地图中进行图像匹配，获取移动机器人在全局地图中的初始位姿。根据本发明的实施方式，可以获取移动机器人上电启动时在全局地图中的绝对位姿，实现移动机器人位姿初始化。进一步地，可以对确定的位姿进行校正。
【专利说明】
移动机器人的位姿确定方法和设备
技术领域
[0001] 本发明的实施方式涉及移动机器人导航中的位姿确定方法，特别是一种基于激光 扫描仪的位姿初始化方法，以及位姿校正方法。
【背景技术】
[0002] 目前所有导航策略无论是室外还是室内导航，都必须解决移动机器人上电启动时 位姿初始化问题，初始位姿的准确性影响后续整个导航的准确性和稳定性。由于目前全球 定位系统(GPS)的成熟，在室外可时刻获得基于地球坐标系的绝对位姿，故上电位姿初始化 在室外已经得到很好解决。但在室内导航领域，目前仍不成熟。
[0003]惯性导航定位技术和激光导航定位技术是目前较为成熟的几种室内导航定位手 段中的两种手段。惯性导航定位精度高，但是存在累计误差，上电初始时刻，需配合外部其 他设备提供地图和初始位姿。基于激光扫描仪的即时定位与地图构建(SLAM)技术可在新环 境下同时建图和定位，第一次创建地图时，初始位姿可为固定值或由其他设备提供，但是在 完成创建地图后，上电初始时刻无法确定移动机器人在已有地图中的初始位姿。对于此类 的导航定位技术，移动机器人上电初期精准的初始位姿定位是首要问题。

【发明内容】

[0004] 有鉴于此，本发明实施方式的所要解决的技术问题之一是为移动机器人确定初始 位姿，即移动机器人的位姿初始化。
[0005] 根据本发明的一个方面，提供一种移动机器人的位姿初始化方法，所述移动机器 人包括激光扫描仪。该方法可以包括：使用所述激光扫描仪，利用即时定位与地图构建 (SLAM)技术创建所述移动机器人在其中导航的环境的全局地图；在所述移动机器人上电 时，使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建针对所述移动机器人 的上电时刻的环境的局部地图；以及将所述局部地图在所述全局地图中进行图像匹配，获 取所述移动机器人在全局地图中的初始位姿。
[0006] 进一步地，所述移动机器人还可以包括惯性导航传感器，所述方法还包括:在所述 移动机器人的位姿发生改变时，利用所述惯性导航传感器计算所述移动机器人在所述全局 地图中的第一位姿;使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于 所述移动机器人的周围环境的局部地图；将所述局部地图在全局地图中进行图像匹配，获 取所述移动机器人在所述全局地图中的第二位姿和与所述第二位姿对应的匹配可信度，其 中所述第二位姿为图形匹配得出的所述移动机器人在所述全局地图中的最佳位姿；以及根 据所述匹配可信度，确定所述移动机器人的位姿为所述第一位姿或者所述第二位姿。
[0007] 进一步地，所述匹配可信度为使用所述激光扫描仪在所述局部地图中进行激光扫 描获得的障碍轮廓在所述全局地图中的映射轮廓的坐标的像素和、并且对所述像素和进行 归一化的结果。
[0008] 进一步地，所述图像匹配可以采用SIFT算法或模板匹配算法。
[0009] 进一步地，在创建所述全局地图和所述局部地图时可以都依赖于所述移动机器人 所包括的电子指南针的航向角来确定移动机器人的初始位姿。
[0010] 进一步地，所述方法还可以包括:在移动机器人位姿初始化或者位姿发生改变时， 根据移动机器人室内定位技术估计移动机器人的当前位姿;访问参考物数据库，所述参考 物数据库包括所述移动机器人配备的摄像头拍摄的、关于至少一个参考物中的每个参考物 的一幅参考图像，以及与所述参考图像关联的、所述摄像头拍摄所述参考图像时所述移动 机器人的相应真实位姿，并且从所述参考物数据库中选择一个真实位姿和相关联的参考图 像，其中所述参考物是室内环境中的静止物体;将所述移动机器人导航至目标位姿，所述目 标位姿的数值是根据移动机器人室内定位技术对于所选择的真实位姿的估计值;利用所述 移动机器人配备的所述摄像头进行图像拍摄，获得拍摄图像;将所述拍摄图像中包括的参 考物图像、即实际参考物图像与所选择的相关联的参考图像中包括的参考物图像、即数据 库参考物图像进行对比;根据对比的结果来确定所述移动机器人室内定位技术产生的位姿 误差；以及利用所确定的位姿误差对根据所述移动机器人室内定位技术所估计的移动机器 人的位姿进行误差校正。
[0011] 根据本发明的一个方面，提供一种移动机器人的位姿初始化设备，所述移动机器 人包括激光扫描仪。该设备可以包括:全局地图创建装置，用于使用所述激光扫描仪，利用 即时定位与地图构建(SLAM)技术创建所述移动机器人在其中导航的环境的全局地图；局部 地图创建装置，用于在所述移动机器人上电时，使用所述激光扫描仪，利用即时定位与地图 构建(SLAM)技术创建针对所述移动机器人的上电时刻的环境的局部地图；以及初始位姿获 取装置，用于将所述局部地图在所述全局地图中进行图像匹配，获取所述移动机器人在全 局地图中的初始位姿。
[0012] 根据本发明的一个方面，提供一种在计算机可读存储介质中实现的计算机程序产 品，所述计算机可读存储介质具有存储于其中的计算机可读程序代码部分，所述计算机可 读程序代码部分被配置为执行根据前述的方法。
[0013] 根据本发明实施方式的技术方案，可以获取移动机器人上电启动时在全局地图中 的绝对位姿，实现移动机器人位姿初始化。可以做到机器人放在任意有效位置点，上电启动 时均可确定其准确的初始位姿，即坐标和航向角。进一步可选地，能够及时发现移动机器人 在室内导航过程中所产生的位姿误差，从而在必要时可以对该位姿误差进行校正。
[0014] 从下文结合附图所做出的详细描述中，本发明的这些和其他优点和特征将变得更 加明显，其中在整个下文描述的若干附图中，类似的元件将具有类似的编号。
【附图说明】
[0015] 图1图示根据本发明的一个实施方式的移动机器人位姿初始化方法的流程图；
[0016] 图2图示根据本发明一个实施例所创建的全局地图；
[0017] 图3图示根据本发明一个实施例所创建的局部地图；
[0018] 图4图示如图3所示的局部地图在如图2所示的全局地图中的匹配示意图；
[0019] 图5图示根据本发明实施方式的一种模块匹配方法的示意图；
[0020] 图6图示如图3所示的局部地图中的最小有效地图图像的示意图；
[0021 ]图7图示移动机器人所包括的激光扫描仪的一种可能布置的示意图；
[0022] 图8图示移动机器人所包括的激光扫描仪的另一种可能布置的示意图；
[0023] 图9图示根据本发明的一个实施方式的移动机器人位姿初始化设备的框图；
[0024] 图10图示现有技术的室内定位和导航技术的方法的流程图；
[0025]图11图不了基于超宽带(UWB)的定位和导航技术的不意图；
[0026] 图12图示根据本发明的实施方式的用于移动机器人定位的方法的流程图；
[0027] 图13图示根据本发明的实施方式的用于移动机器人定位和导航的方法的流程图；
[0028] 图14A、图14B和图14C图示实际参考物图像相对于数据库参考物图像所可能发生 变化的不意图；
[0029] 图15图示实际参考物图像与数据库参考物图像匹配的示意图；
[0030] 图16图示了移动机器人拍摄画面中的数据库参考物图像和实际参考物图像的可 能的偏差；
[0031] 图17图示根据本发明的一个实施方式的移动机器人位姿确定方法的流程图；
[0032] 图18图示根据本发明的一个实施方式的移动机器人位姿确定方法的流程图；
[0033] 图19图示根据本发明实施方式的一种激光导航测量原理结构图；以及 [0034]图20图示根据本发明的一个实施方式的移动机器人位姿确定方法的流程图。
【具体实施方式】
[0035] 下文将参考附图更完整地描述本公开内容，其中在附图中显示了本公开内容的实 施方式。但是这些实施方式可以用许多不同形式来实现并且不应该被解释为限于本文所述 的实施方式。相反地，提供这些实例以使得本公开内容将是透彻和完整的，并且将全面地向 本领域的熟练技术人员表达本公开内容的范围。
[0036] 参考图1，其图示了根据本发明的实施方式的移动机器人的位姿初始化方法100的 流程图。移动机器人包括激光扫描仪。
[0037] 在步骤S110,使用激光扫描仪，利用即时定位与地图构建(SLAM)技术(下文也称为 激光SLAM技术)创建移动机器人在其中导航的环境的全局地图。移动机器人在导航环境中 的位姿包括坐标和航向角，用(x，y，0)来表示。
[0038] 在一个未知环境(比如室内环境)下，采用激光SLAM技术对当前环境进行第一次建 图，可以设定地图大小(地图大小应比实际的室内范围大)为2048*2048。建图时移动机器人 的假定初始位姿比如可固定为（1024,1024，0)。可以自主地导航移动机器人使其走遍室内 范围，也可以人工引导机器人走遍整个室内范围，创建全局地图。创建的地图可以为栅格地 图，如图2所示，其示出了作为一个实施例的所创建的全局地图，其中障碍的像素值为0,无 障碍的像素值为255,未观测到的像素值为127。可以将创建好的地图进行保存在移动机器 人的存储器中，或者移动机器人外部的存储设备中、在后续的步骤中移动机器人可以与该 外部存储设备中进行无线或有线通信来获取所保存的全局地图。
[0039] 在建立了上述地图的情况下，则此后移动机器人的定位、导航、路径规划等均可以 以此地图为全局地图，定位点以此图的坐标为绝对位姿。
[0040] 在步骤S120,在移动机器人上电时，使用激光扫描仪，利用即时定位与地图构建 (SLAM)技术创建针对移动机器人的上电时刻的环境的局部地图。
[0041] 在移动机器人上电重新启动时，可以设置移动机器人的假定初始位姿为（1024， 1024,0)，开启激光SLAM建图功能，使得移动机器人在原位置自行旋转一周，形成较大的、详 细的、360度的局部地图，如图3所示，其示出了作为一个实施例的所创建的局部地图。由于 上电时刻移动机器人位姿可能为任意值，故匹配时需考虑360度范围的航向角情况下在地 图中的位置。应当理解，要创建360度的局部地图，需要的移动机器人旋转的角度和移动机 器人上布置的激光扫描仪数量以及激光扫描仪视野范围有关。
[0042]在步骤S130,将局部地图在全局地图中进行图像匹配，获取移动机器人在全局地 图中的初始位姿。可以将移动机器人上电重新启动时所创建的360度的局部地图与全局地 图采用图像处理方法进行图像匹配，得到其在全局地图中的位置和方向，如图4所示，其示 出了如图3所示的局部地图在如图2所示的全局地图中的匹配示意图。从而，根据图像匹配 的结果，可以获取移动机器人的在全局地图中的初始位姿，其为绝对位姿。
[0043] 激光SLAM所创建的地图可以为二维栅格灰度图像，可如图2、图3所示，地图存在复 杂性、毛刺、粗糙、不工整等特点，尤其此类二维图像的特征点不是十分明显。同时在创建局 部底部的初始时刻，激光所测局部距离信息点为离散的、局部性强、特征点残缺等特点，直 接与原始的全局地图进行匹配易出现匹配错误或无法得到正确的匹配。进一步地，为了实 现局部地图与全局地图的精准匹配，可以采用的具体图像匹配方式包括但不限于尺度不变 特征转换（SIFT，Scale-invariant feature transform)算法和模板匹配算法，还可以使用 直方图匹配、形状匹配、蒙特卡洛等匹配算法。
[0044] SIFT算法是用于图像匹配的经典算法之一，用来侦测与描述影像中的局部性特 征，它在空间尺度中寻找极值点，并提取出其位置、尺度、旋转不变量。此算法由David Lowe 在1999年所发表，2004年完善总结。根据本发明的实施方式，局部地图和全局地图采用完全 相同的SLAM算法绘制而成，从图2、图3、图4可以看出，图2所示局部地图是图1所示的全局地 图中的一部分，但是位置和朝向随机（由于全局地图是扫描仪的n次扫描结果相叠加的结 果，而局部地图是1次扫描的结果，所以肉眼未必能够发现局部地图在全局地图中的位置和 朝向，但计算机视觉算法SIFT可以实现）。故图像处理中SIFT算法非常适用于旋转、平移的 两幅图像匹配，即本发明实施方式的场景。图3相对于图2进行SIFT特征提取、并进行匹配， 得出旋转角度为9t、平移向量为(xt，yt)时为最佳匹配，如图4所示。由于两幅图建图时的移 动机器人假定初始位姿设置为相同，比如均为（1024，1024,0)，而其中图2作为绝对坐标系， 图3中的假定初始位姿（1024,1024,0)实际为图2中当前位姿，经旋转与平移即可得到在图2 中的绝对坐标。从而，获取移动机器人在图2的全局地图中的初始位姿POSE为：
[0045] POSE=(lO24，lO24,O) + (xt，yt，0t)
[0046] =(l〇24+xt，l〇24+yt，0t)。
[0047] 模板匹配也是用于图像匹配的经典算法之一，用于判断数字图像的模板是否匹 配，即模板与原图像进行比较，以确定在原图像中是否存在与该模板相同或相似的区域，若 该区域存在，还可确定其位置，认为模板与原图像匹配。根据本发明的实施方式，局部地图 和全局地图采用完全相同的SLAM算法绘制而成，从图2、图3、图4可以看出，图2所示局部地 图是图1所示的全局地图中的一部分，但是位置和朝向随机。模板匹配常用的方法有平方差 匹配法、相关匹配法、相关系数匹配法、归一化平方差法、归一化相关匹配法模板等等。
[0048] 在一个实施例中，可以采用一种相关匹配法与归一化相关匹配法相结合，匹配相 似度则为两幅图像的相关数值。相关数值越大，则越相似。匹配原理示意图如图5所示。图5 从左到右依次图示了将局部地图旋转n度，找出最小有效地图，获取偏移坐标(x_off set_n， y_of f set_n);提取最小有效地图，作为模板;获得最佳匹配位置(local_x_n，local_y_n)。 参考图5,一种示例性匹配算法的具体步骤可以包括:1.提取图3中最小有效地图图像，如图 6,即将图3进行裁剪后的地图作为模板;2.裁剪后的地图模板（图6)与原局部地图图像（图 3)的水平偏移为(xjffsetJdjffsetJ)』.采用归一化模板匹配算法，找到图2中与图6 最佳匹配位置，获得在图2中图6左上角的坐标（l 〇Cal_X_l，l〇Cal_y_l)，同时获得最佳匹配 位置的未归一化相似度result」。4.将图3顺时针旋转一度，重复前述步骤1、步骤2、步骤3， 直到遍历完360度为止。5.获得360个最佳匹配位置，即局部地图遍历360度，每一度均存在 一个最佳匹配位置，提取有效地图的水平偏移为(x_off set_n，y_off set_n)，对应的匹配位 置的模板的左上角坐标为（l〇cal_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.图3中（1024,1024,0)位姿为上电初 始位姿，则根据其在图2中的映射关系可以获取移动机器人在图2的全局地图中的初始位姿 POSE 为：
[0049] P〇SE=( 1024,1024,0)-(x_ofTset_R，y_ofTset_R) + (0,0，R) + (l〇cal_x_R，local_ y_R,0)
[0050] =(1024-x_offset_R+local_x_R,1024-y_)〇
[0051] 在获取了移动机器人在全局地图中的初始位姿后，便可利用惯性导航定位技术、 激光导航定位技术或者其他适当的导航定位技术或它们的组合对移动机器人在已有全局 地图中进行精确定位，实现自动导航功能。
[0052]在一个实施例中，移动机器人可包括2个激光雷达作为激光扫描仪，用于创建全局 地图和360度局部地图。图7和图8图示移动机器人所包括的激光扫描仪的可能的布置的示 意图。如图7所示，2个激光雷达分别安装在移动机器人的前方和后方，从而可直接观测正前 方180度和正后方180度范围的距离信息。从而，在创建360局部地图时，无需移动机器人进 行360度的旋转，只需旋转90度。如图8所示，2个激光雷达选用可视角为270度的激光雷达， 分别被安装在移动机器人的前方左角和后方右角，分别可以观测正前方和左侧以及正后方 和右侧。从而，可直接观测机器人周围360度范围距离信息，在创建360局部地图时，无需移 动机器人进行旋转。
[0053] 在一个实施例中，移动机器人可包括电子指南针用于全局地图和局部地图的创 建。在未知环境下，采用激光SLAM技术对当前环境进行第一次建图，可以设定地图大小(地 图大小应比实际的室内范围大)为2048*2048。建图时移动机器人的初始位置比如可固定为 (1024,1024)，可按照电子指南针的航向角0〇作为初始航向角，故初始位姿为（1024,1024， 9〇)。人工引导走遍整个室内范围，创建的完整的栅格地图并保存。在移动机器人上电重新 启动而创建局部地图时，也可以采用电子指南针航向角作为初始航向角。
[0054]由于每次建图都是以电子指南针航向角为移动机器人的初始航向角，但由于电子 指南针自身误差，同时不同位置处可能受外界磁场变化也会引起一定误差，故上电重启时 电子指南针的航向角不能直接作为局部地图中的初始航向角，但一定在电子指南针航向角 附近处。设电子指南针最大误差为正负15度，则匹配时仅需考虑30度范围内航向角的地图 匹配，大大提高匹配速度和准确率。将局部地图与全局地图采用图像处理方案进行图像匹 配，得到当前在原始地图中的初始位姿(绝对位姿）。由于全局地图和局部地图的创建，都依 靠电子指南针的航向角确定移动机器人的初始航向角，从而，在采用诸如SIFT和模板匹配 两种算法进行地图匹配时，航向角可以约束在正负15度范围内进行匹配，因此可以极大地 降低局部地图和全部地图匹配的复杂度。
[0055] 图9示意性图示根据本发明实施方式的移动机器人的位姿初始化设备900的框图。 设备900可以包括:全局地图创建装置910,用于使用激光扫描仪，利用即时定位与地图构建 (SLAM)技术创建移动机器人在其中导航的环境的全局地图；局部地图创建装置920,用于在 移动机器人上电时，使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建针对移动 机器人的上电时刻的环境的局部地图；以及初始位姿获取装置930,用于将局部地图在全局 地图中进行图像匹配，获取移动机器人的初始位姿。
[0056] 应当理解，设备900中记载的各个装置与参考图1描述的方法100中的各个步骤相 对应。由此，上文针对方法100描述的操作和特征同样适用于设备900及其中包含的装置，重 复的内容在此不再赘述。
[0057] 根据本发明的实施方式的移动机器人的位姿初始化方法和设备，可以获取移动机 器人上电启动时在全局地图中的绝对位姿，实现移动机器人位姿初始化。可以做到无论机 器人放在任意位置点，上电启动时均可确定其准确的初始位姿。
[0058] 另一方面，随着移动机器人技术的快速发展和日趋成熟，移动机器人在家庭和公 共场所等室内环境的应用也越来越广泛。基于移动机器人的各种服务和应用要求移动机器 人能够实时了解自身的位姿信息，包括自身位置和航向，然后可以按照规划路径准确到达 目的地完成指定任务。因此，室内定位是移动机器人领域的关键技术之一。
[0059] 室内定位目前出现了各种技术，如超宽带技术、基于无线局为(WLAN)定位技术、激 光测距定位技术、基于惯性导航的定位技术、基于里程计的定位技术等。而每种定位技术都 存在各自的缺点和局限性，大多数方案采用多种定位技术的组合导航。但是由于每种定位 技术所依赖的传感器本身精度限制和周围环境干扰，导致室内定位不可避免地存在位置和 航向误差。
[0060] 在进一步的实施方式中，可以进一步对移动机器人初始化后的位姿或者位姿发生 改变后的位姿进行位姿误差校正。下面结合图10-图16来描述具体的误差校正过程。
[0061] 图10图示了现有技术的室内定位和导航技术的方法1000的流程图。
[0062]步骤S1010,室内地图创建。室内地图的创建可以采用卷尺和激光测距仪等方法， 测量墙壁的长度和厚度以及相对坐标原点的位置，基于直角坐标系建立室内地图数据库。 [0063]步骤S1020,路径规划。路径规划一般是指机器人在有障碍物的环境中，按照一定 的评价标准，寻找一条从当前位置到目标位置的无碰撞路径。移动机器人导航引擎在得到 目标位置与当前位置信息后，就需要根据地图，计算出最优的路径以及可能的备选路径，将 移动机器人导航到目的地。常见的路径规划算法例如包括A*和Dijkstra算法。
[0064] 步骤S1030,室内导航。移动机器人导航引擎使用所规划的路径将移动机器人导航 到目标位置。
[0065] 移动机器人导航引擎可以采用的定位和导航技术例如可以是基于超宽带(UWB)的 定位和导航技术、基于惯性的定位和导航技术、基于无线局域网(WLAN)的定位和导航技术、 激光测距定位和导航技术、基于视觉导航的定位和导航技术、基于超声波测距的定位和导 航技术、或者它们中的一项或多项的结合。本领域技术人员应当能够理解这些定位和导航 技术的基本原理和位姿误差产生的原因，然而考虑到本文的完整性，下面仅以超宽带(UWB) 技术和惯性定位和导航技术为例做个简单说明。
[0066]超宽带可以测量基站和标签之间的距离信息。将标签安装到移动机器人上，当标 签同时测量到三个及以上基站的距离信息时，采用三边测量法就可以计算出移动机器人的 位置和航向。图11图示了基于超宽带(UWB)的定位和导航技术的示意图。如图11所示，具体 计算步骤可以如下。
[0067] 1)基站六、8、(：的位置为已知，分别为(13,73)、（11 )，71))、（1。，7。），利用基站发射的超 宽带信号测量的其距离标签T的距离分别为ra、rb和r c;
[0068] 2)标签T的位置为(Xt，yt)。则根据步骤1)的信息，可得出以下公式：
[0069] (xf - xa) + [yt - yj = i；2
[0070] + (yt - = l；2
[0071] (A _ 义)」+ k = J:,」
[0072 ] 3)由步骤2)中公式，可计算出标签T的位置(xt，yt)为：
[0075] 4)根据当前时刻标签T的位置(Xt，yt)和上一时刻标签T的位置( XtQ，ytQ)，可计算标 签T的航向W为：
[0077] 可见，在超宽带室内定位中，引起位姿误差的因素有两个，一是UWB信号的传播效 应，包括非视距传播效应、多径传播效应及远近效应等，二是由于当前时刻标签位置的计算 依赖于前一时刻标签位置所带来的累积误差。
[0078] 惯性定位和导航技术是通过陀螺仪、加速计、里程计等传感器来进行移动机器人 的定位和导航。陀螺仪测量移动机器人的角速度，里程计测量移动机器人的移动距离，陀螺 仪/里程计可以计算移动机器人的相对变化位置。具体步骤如下：
[0079] 1)在室内布置多个超宽带基站，并记录基站在室内地图下的位置信息；
[0080] 2)当组合导航未初始化位置和航向时，如果标签测量到的基站距离信息小于三 个，则移动机器人自由行走，直到测量到的基站距离信息不少于三个；
[0081] 3)当标签测量到三个及以上基站数据时，移动机器人根据标签和基站的距离信息 计算移动机器人的位置和航向，完成组合导航的位置和航向初始化；
[0082] 4)移动机器人根据当前时刻的位置和航向，基于陀螺仪的角速度数据和里程计的 距离数据计算下一时刻的位置和航向；
[0083] 5)当标签测量到三个及以上基站数据时，计算出移动机器人的位置和航向，并与 步骤4)所得位置和航向按照设置的比例参数进行融合，并作为移动机器人的真实位置和航 向；
[0084] 6)当标签未测量到三个基站数据时，步骤4)所得位置和航向即为移动机器人的真 实位置和航向。
[0085] 可见，惯性定位和导航技术所使用的传感器的精度会产生即时误差，依赖于前一 时刻位姿来计算当前时刻位姿会带来累积误差。
[0086] 参考图12,其图示根据本发明的实施方式的用于移动机器人定位的方法300的流 程图。方法300可以用于确定移动机器人室内定位方法的位姿误差，并且可选地，可以进行 位姿误差校正。位姿（也称为定位)可以包括移动机器人的位置和航向，位姿误差可以包括 位置误差和航向误差。应当理解，方法300可以结合在方法100的描述所提及的任何一种定 位和导航技术而使用。
[0087] 步骤S310,根据移动机器人室内定位技术估计移动机器人的当前位姿。
[0088] 步骤S320,访问参考物数据库，所述参考物数据库包括移动机器人配备的摄像头 拍摄的、关于至少一个参考物中的每个参考物的一幅或多幅参考图像，以及与所述参考图 像关联的、所述摄像头拍摄参考图像时移动机器人的真实位姿，其中所述参考物是用来在 其中定位移动机器人的环境中的静止物体，并且从所述参考物数据库中选择一个真实位姿 和相关联的参考图像。
[0089] 在一个实施例中，可以从参考物数据库中选择与所选择的当前位姿所需距离最短 的真实位姿。在一个实施例中，可以从参考物数据库中选择从当前位姿导航到所需时间最 短的真实位姿。在一个实施例中，可以选择在所需距离和所需时间两个方面较为折中的真 实位姿。
[0090] 步骤S330，将移动机器人导航至所选择的真实位姿，即目标位姿，所述目标位姿是 根据所述移动机器人室内定位技术所估计出的。
[0091] 步骤S340,利用移动机器人配备的摄像头进行图像拍摄，获得拍摄图像。应当注 意，该拍摄图像中需要包括参考物图像，该参考物也出现在所选择的相关联的参考图像中； 否则，表明移动机器人室内定位技术的累积误差过大了，使得参考物不能出现在移动机器 人配备的摄像头的视野中了，至少应当在累积误差积累到出现这种情况之前进行误差校 正。在一个实施例中，可以据出现这种情况来定义误差校正阈值。
[0092] 步骤S350,将所述拍摄图像中包括的参考物图像(也称为实际参考物图像)与所选 择的相关联的参考图像中包括的参考物图像(也称为数据库参考物图像)进行对比。对比可 以包括形状和/或尺寸方面的对比。形状对比是指判断实际参考物图像相对于数据库参考 物图像是否存在变形，比如向左倾斜、向右倾斜，这是由于拍摄时的摄像头的方向不同（表 明机器人的航向不同）所引起的。尺寸对比是指判断实际参考物图像相对于数据库参考物 图像在形状相同的前提下是否存在大小变化，比如尺寸偏小、尺寸偏大，这是由于拍摄时的 摄像头距离参考物的距离不同所引起的。
[0093] 步骤S360,根据对比的结果来确定移动机器人室内定位技术产生的位姿误差。
[0094] 在可选的步骤S370,必要时，可以利用所确定的位姿误差对步骤S310中所估计的 移动机器人的当前位姿进行误差校正。可以只在确定的位姿误差超过预设的阈值时，才执 行此校正步骤。例如，该阈值可以是进行了预设次数的定位和导航，比如执行了 3次惯性导 航后执行一次位姿误差校正。
[0095] 进一步地，在步骤S320之前，方法300还可以包括步骤S315,确定是否需要对步骤 S310中所估计的移动机器人的当前位姿进行误差确定(未示出）。只有在估计当前位姿的误 差可能超过误差阈值时，才执行步骤S320及后续进行，进行位姿误差确认。例如，该误差阈 值可以是进行了预设次数的定位和导航，比如执行了 3次惯性导航后执行一次位姿误差确 定，还可以是航向误差超过了预设的航向误差阈值、和/或位置误差超过了预设的位置误差 阈值;或者，该误差阈值可以依据所采用移动机器人室内定位技术而定;或者，该误差阈值 可以依据定位所使用传感器的精度而定。
[0096]可选地，方法300还可以包括建立参考物数据库的步骤。应当理解，无需针对每次 误差确定或者误差校正的方法300的流程都执行建立参考物数据库的步骤。预先建立的参 考物数据库也可以为家庭内的不同的移动机器人所共用。可以将参考物数据库存储在移动 机器人中，也可以将参考物数据库存储在移动机器人之外的服务器中，在步骤S320中，移动 机器人需要与该服务器通信来获取从所述参考物数据库中选择的真实位姿和相关联的参 考图像。
[0097]参考图13,其图示了根据本发明的实施方式的用于移动机器人定位和导航的方法 400的流程图。方法400可以理解为是将图11所示的位姿误差确定和校正方法应用于图10所 示的移动机器人定位和导航方法的一个具体实例。
[0098] 步骤S402,室内地图创建。
[0099]步骤S404,建立参考物数据库，具体建立步骤可以如下进行。
[0100] 1)可以选择固定在墙壁上的电灯开关、消防警钟、电视等作为参考物。所选参考物 不局限于以上物体，所有固定在墙壁上的易识别物体和标记均在所述范围之内；或者，还例 如可以是固定摆放的家具（比如电视、饮水机等)或者家具上的标记(比如Logo、电子家具的 开关按键)等。
[0101] 2)将移动机器人调整到处于参考物前方，开启摄像头拍摄参考物，存储参考物图 像至参考物数据库，并用卷尺或激光测距仪等准确测量此时(进行拍摄时)移动机器人的位 姿，包括位置Pt rue和航向Atrue，建立参考物图像与拍摄时的机器人位姿Ptrue和航向A true-一 对应的参考物数据库记录。参考物数据库不局限某一个参考物，可建立多个参考物图像与 拍摄位置和航向的数据库，且同一幅图像中可同时包含多个参考物。
[0102] 3)以多个角度拍摄同一参考物，和/或以一个或多个角度拍摄不同的参考物，按照 步骤2)建立参考物数据库。
[0103] 步骤S410，根据移动机器人室内定位技术估计移动机器人的当前位姿。
[01 04]步骤S420，访问参考物数据库，并且从所述参考物数据库中选择一个真实位姿和 相关联的参考图像。
[0105]步骤S422,路径规划。比如，当监测到定位传感器出现故障、定位系统受到环境干 扰等情况时，这可能会导致定位或定向误差超过阈值，移动机器人需要利用参考物进行定 位和定向误差确定和修正。移动机器人可以根据当前位姿从参考物数据库中选择距离最近 的参考物图像，并将所对应的拍摄位置和航向作为目标定向，根据室内地图计算出从当前 位置到目标点的最佳路径，避开墙壁等各种障碍物，指导移动机器人按照规划路径行驶。
[0106] 步骤S424,室内导航。移动机器人导航引擎使用步骤S422中所计算的路径将移动 机器人导航到目标定向。
[0107] 步骤S430，将移动机器人导航至所选择的真实位姿，即目标位姿，所述目标位姿是 根据所述移动机器人室内定位技术所估计出的。
[0108] 步骤S440,利用移动机器人配备的摄像头进行图像拍摄，获得拍摄图像。
[0109] 移动机器人到达目的定向后，可以打开摄像头进行数据采集，将摄像画面作为图 像存储。由于室内导航存在误差，移动机器人的位置和航向与预期存在差距，导致拍摄的实 际参考物与数据库参考物相比，可能存在变形、尺寸偏小和尺寸偏大等问题。
[0110] 步骤S450,将所述拍摄图像中包括的参考物图像(也称为实际参考物图像)与所选 择的相关联的参考图像中包括的参考物图像(也称为数据库参考物图像)进行对比。图14A、 图14B和图14C图示摄像头拍摄的实际参考物图像相对于数据库参考物图像所可能发生的 变化的示意图，分别图示了实际参考物图像发生变形、尺寸偏大、尺寸偏小的情形。
[0111]步骤S460,根据对比的结果来确定移动机器人室内定位技术产生的位姿误差，具 体可以包括步骤S4602和S4604。
[0112] 在步骤S4602,根据对比的结果调整所述移动机器人的航向，使得移动机器人配备 的摄像头所拍摄的图像中的参考物图像与所选择的相关联的参考图像中包括的参考物图 像趋于一致。
[0113] 根据摄像头采集的实际参考物图像和数据库参考物图像进行对比，分析两者的匹 配情况，判断实际参考物的状态，包括变形、尺寸偏小和尺寸偏大三种。当移动机器人位置 偏左、偏右或拍摄航向与数据库的移动人定向不一致时，会导致参考物变形；当移动机器人 位置偏前或偏后时，会导致参考物尺寸偏大或偏小。
[0114] 可以通过调整移动机器人的位置和航向，校正参考物的拍摄效果，实现参考物的 拍摄效果与数据库参考物的某一个图像一致。
[0115] 在步骤S4604,确定所述移动机器人所调整的位姿变化，即为所述移动机器人室内 定位技术产生的当前位姿误差，包括位置误差Pm和航向误差Am。
[0116] 步骤S470,利用所确定的位姿误差对步骤S410中所估计的移动机器人的当前位姿 进行误差校正。在步骤S460中计算出位置误差P err和航向误差Aerr后，可以对步骤S410中所 估计的位姿，即位置P1和航向A1，进行校正，得到移动机器人的校正后的位姿，位置Pi'和 Ai'：
[0117] Pl^Pl-Perr
[0118] A'i = Ai-Aeir
[0119] 在步骤S360和步骤S460中，可以有多种方式确定移动机器人室内定位技术产生的 位姿误差。
[0120] 在第一实施方式中，可以只采用参考物数据库中的参考物的一幅图像，通过调整 移动机器人的位置和航向，使拍摄的实际参考物与数据库参考物达到最佳匹配度来进行误 差确定，可以执行如下步骤。
[0121 ]步骤Sla，设置移动机器人导航引擎使得移动机器人左右移动的最小距离为d，左 右旋转最小角度为9,预先设定在进行位姿误差确定时要进行的移动次数为左右各为m，要 进行的旋转次数左右各为n。移动机器人未进行调整前的位置为Po，航向为Ao。
[0122] 步骤Sib，在移动机器人的当前位姿(Po，Ao)，保持移动机器人位置不动，将航向依 次向左旋转0*9、1*9、…、n*9拍摄参考物，并相应计算实际参考物图像与数据库参考物图像 的匹配度So、Si、…、S n。然后，向右旋转n*0，回到航向Ao。接着，将机器人的航向依次向右旋 转1*0、2*0、…、n*0拍摄参考物，并相应计算实际参考物图像与数据库参考物图像的匹配度 Sn+1、Sn+2、…、S2n。然后，向左旋转n*0，回到航向Ao。则，该位姿(Po，Ao)所拍摄的实际参考物图 像与数据库参考物图像的最高匹配度为DoiMAXtS^Si，…，S 2n}，记录该位置的最高匹配度 Do、和与Do对应的左右旋转的航向Yo (Yo = j*0，j属于{-n，…，n}中的某个值），其中j表示取 得最高匹配度Do的航向左右旋转的次数。还同时记录移动机器人左右移动的距离Lo(L() = i* (14属于{-111，一，111}中的某个值）。其中1表示移动机器人相对于位姿(？()4())进行左右移动的 次数，此时，由于移动机器人未进行左右移动，所以i = 〇。
[0123] 步骤Sic，移动机器人依次向左移动l*d、2*d、…、m*d，并在每个位置执行步骤Sib， 计算并记录相应的最高匹配度Di以及对应的左右移动距离和航向，从而，有池山，YU，{D 2， L2，Y2}，'??，{ Dm，Lin，Yin } 〇
[0124] 步骤Sld，移动机器人回到当前位姿(Po，AQ)。然后，移动机器人依次向右移动l*d、 2*d、…、m*d，并在每个位置执行步骤Sib，计算并记录相应的最高匹配度Di以及对应的左右 移动距离和航向，从而，有{D-hL-hY-U，{D- 2，L-2，Y-2}，…，{D-m，L-m，Y- m}。
[0125] 步骤Sle，在不考虑尺寸问题的情况下，实际参考物图像与数据库参考物图像的最 佳匹配度为D=MAX{D-m，…，，…，D m}。根据D所对应的左右移动距离L和左右旋转航 向Y调整移动机器人，该状态下所拍摄的实际参考物图像与数据库参考物图像只存在尺寸 不一致问题，不存在变形。
[0126] 步骤Slf，若此时实际参考物图像与数据库参考物图像尺寸也相等，则参考物图像 的特征匹配完成，如图15所示，其示出了实际参考物图像与数据库参考物图像尺寸刚好匹 配的情形。记录移动机器人此时的位姿为(丹，心），为室内定位系统的位置和航向输出。则此 时，位置误差IW = Pl-PtrUe，航向误差和Atrue为与数据库参考物图像对应 的参考物数据库中的移动机器人位姿，包括位置和航向。
[0127] 若此时实际参考物图像比数据库参考物图像尺寸小，则调整移动机器人前进，直 到两者尺寸相等，参考物特征匹配完成，如图15所示。记录移动机器人此时的位姿为(Pi， Ai )，为室内定位系统的位置和航向输出。则此时，位置误差Perr = Pi-Ptrue，航向误差Aerr = 和Atrue5为与数据库参考物图像对应的参考物数据库中的移动机器人位姿，包 括位置和航向。
[0128] 若此时实际参考物图像比数据库参考物图像尺寸大，则调整移动机器人后退，直 到两者尺寸相等，参考物特征匹配完成，如图15所示。记录移动机器人此时的位姿为(Pi， Ai )，为室内定位系统的位置和航向输出。则此时，位置误差Perr = Pi-Ptrue，航向误差Aerr = 和Atrue5为与数据库参考物图像对应的参考物数据库中的移动机器人位姿，包 括位置和航向。
[0129] 在第一实施方式中，需多次调整移动机器人，比较耗时，且对移动机器人进行左右 移动的调整比较麻烦。为此，在第二实施方式中，可采用数据库中一个参考物的多幅图像进 行特征匹配，避免左右移动。
[0130] 在第二实施方式中，参考物数据库包括一个参考物的多幅图像，该多幅图像包括 移动机器人在一个拍摄位置拍摄的一幅图像、和在该位置左右各移动m次拍摄的2m幅图像。 机器人每次能够左右移动的距离是确定的，例如为d。在第二实施方式中，采用数据库的多 幅参考物图像进行匹配，通过调整移动机器人左右旋转(调整航向）和前后移动实现参考物 特征匹配，使拍摄的实际参考物与数据库参考物达到最佳匹配度来进行误差确定，可以执 行如下步骤。
[0131] 步骤S2a，保持移动机器人位置不动，调整移动机器人左右旋转（依次改变航向 角），使实际参考物图像与多幅数据库参考物图像中的某幅图像的匹配度最高，该状态下所 拍摄的实际参考物图像与数据库参考物图像只存在尺寸不一致问题，不存在变形。
[0132] 步骤S2b，当实际参考物图像与数据库参考物图像尺寸相等时，参考物特征匹配完 成；当实际参考物图像尺寸比数据库参考物图像尺寸小时，调整移动机器人前进，直到两者 尺寸相等，参考物特征匹配完成；当实际参考物图像尺寸比数据库参考物图像尺寸大时，调 整移动机器人后退，直到两者尺寸相等，参考物特征匹配完成。在后两种情况下，将移动机 器人移动到参考物数据库中的与最匹配的数据库参考物关联的移动机器人拍摄该图像时 的位置。记录移动机器人最后的位姿为(P^AO,为室内定位系统的位置和航向输出。则此 时，位置误差IW = Pl-PtrUe，航向误差和Atrue为与数据库参考物图像对应 的参考物数据库中的移动机器人位姿，包括位置和航向。
[0133] 在第三实施方式中，由于室内地面通常是平坦的，可以假设移动机器人摄像头的 高度是固定不变的，则参考物在摄像画面中的相对高度是不变的，只存在左右偏差。图16图 示了移动机器人拍摄画面(也称为摄影画面）中的数据库参考物图像和实际参考物图像的 可能的偏差。因此，有可能通过偏差距离来反推航向误差，避免移动机器人旋转，减少调整 过程，具体步骤如下。
[0134] 步骤S3a，数据库参考物图像和实际参考物图像距离摄影画面左边缘的距离分别 为dl和d2。所述距离可以不局限于左边缘距离，还可以是右边缘距离、或者左边缘和右边缘 的结合。
[0135] 步骤S3b，假设移动机器人旋转角度与摄影画面中的距离系数为f，系数f可以用移 动机器人拍摄数据库参考物图像时距离参考物的距离和航向来表示，可以将其与数据库参 考物图像相关联的存储在参考物数据库中，则移动机器人的航向误差为：
[0137] 经过航向误差Aerr校正后所拍摄的实际参考物图像与数据库参考物图像只存在尺 寸不一致问题，不存在变形。
[0138] 步骤S3c，当实际参考物图像与数据库参考物图像尺寸相等时，参考物特征匹配完 成；当实际参考物图像尺寸比数据库参考物图像尺寸小时，调整移动机器人前进，直到两者 尺寸相等，参考物特征匹配完成；当实际参考物图像尺寸比数据库参考物图像尺寸大时，调 整移动机器人后退，直到两者尺寸相等，参考物特征匹配完成。位置误差P err可以采用第一 或者第二实施方式的确定方式。
[0139] 在第三实施方式中，可以通过实际参考物图像和数据库参考物图像距离摄影画面 边缘的距离差来计算航向误差，移动机器人无需左右旋转，但仍需前后调整来计算位置误 差。
[0140]在第四实施方式中，由于参考物的尺寸与摄像头和参考物的距离有关，因此可根 据参考物尺寸差来计算位置误差，具体步骤如下。
[0141]步骤S4a，数据库参考物图像和实际参考物图像距离摄影画面左边缘的距离分别 为dl和d2。所述距离可以不局限于左边缘距离，还可以是右边缘距离、或者左边缘和右边缘 的结合。
[0142]步骤S4b，假设移动机器人旋转角度与摄影画面中的距离系数为f，则移动机器人 的航向误差为：
[0144] 经过航向误差Aerr校正后所拍摄的实际参考物图像与数据库参考物图像只存在尺 寸不一致问题，不存在变形。
[0145] 步骤S4c，参考物数据库中参考物尺寸（比如高度)为hi，实际参考物尺寸（比如高 度)为h2,摄像头与参考物的距离和尺寸的系数为V，系数v可以用移动机器人拍摄数据库参 考物图像时距离参考物的距离来表示，可以将其与数据库参考物图像相关联的存储在参考 物数据库中，则移动机器人的位置误差为：
[0147] 参考图17,其图示了图根据本发明的一个实施方式的移动机器人位姿确定方法 1700的流程图。移动机器人包括激光扫描仪和惯性导航传感器。
[0148] 在步骤S1710,在移动机器人的位姿发生改变时，利用惯性导航传感器计算移动机 器人在全局地图中的第一位姿。全局地图可以是针对移动机器人在其中移动的环境而预先 创建的。使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术、针对移动机器人在其 中导航的环境而预先创建的。
[0149] 惯性导航传感器可以包括陀螺仪、电子指南针、加速计和里程计中一种或多种。陀 螺仪获得机器人可以旋转角度，加速计或里程计可以获取行走距离，电子指南针可以获取 机器人绝对航向角。惯性导航的基本原理为:设移动机器人t时刻位姿为( Xt，yt，0t)，例如采 用陀螺仪和里程计传感器获得角度变化信息、前行距离，可以推出t + 1时刻的测量位姿 (xt+i，yt+i，Qt+i) 〇
[0150] 在步骤S1720,使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于所 述移动机器人的周围环境的局部地图。
[0151] 在步骤S1730,将局部地图在全局地图中进行图像匹配，获取移动机器人在全局地 图中的第二位姿、和与第二位姿对应的匹配可信度。第二位姿为图形匹配得出的移动机器 人在全局地图中的最佳位姿。可以采用的具体图像匹配算法可以包括但不限于，尺度不变 特征转换（SIFT，Scale-invariant feature transform)算法和模板匹配算法，还可以使用 直方图匹配、形状匹配、蒙特卡洛等匹配算法。
[0152] 在步骤S1740,根据所获取的匹配可信度，确定移动机器人的位姿为第一位姿或者 第二位姿。匹配可信度表示第二位姿的准确度，并且其值越大，表明第二位姿越精确，从而 有，当所获取的匹配可信度大于第一阈值时，确定移动机器人的位姿为第二位姿，并且当所 获取的匹配可信度小于或者等于第一阈值时，确定移动机器人的位姿为第一位姿。
[0153] 参考图18,其图示了根据本发明的一个实施方式的移动机器人位姿确定方法1800 的流程图。移动机器人包括激光扫描仪和惯性导航传感器。
[0154] 在步骤S1810,判断移动机器人是否首次出现在新环境中，若判断结果为是，则方 法1800前进至步骤S1820,否则，方法1800前进至步骤S1830。
[0155] 在步骤S1820,使用激光扫描仪，利用即时定位与地图构建(SLAM)技术创建全局地 图，并且在移动机器人上电时对移动机器人在该全局地图中的位姿进行初始化。
[0156] 后续步骤31830、31840、31850和31860涉及在移动机器人的位姿发生改变时，确定 移动机器人在全局地图中的精确位姿，其分别与参考方法1700所描述的步骤S1710、S1720、 S1730 和 S1740 相同。
[0157] 惯性导航定位技术和激光导航定位技术是目前较为成熟的几种室内导航定位手 段中的两种手段，二者都需要依赖于预先设定的关于环境的全局地图。另外，惯性导航定位 精度高，但是存在累计误差，上电初始时刻，需配合外部其他设备提供地图和初始位姿。基 于激光扫描仪的即时定位与地图构建(SLAM)技术可在新环境下同时建图和定位，第一次创 建地图时，初始位姿可为固定值或由其他设备提供，但是在完成创建地图后，上电初始时刻 无法确定移动机器人在已有地图中的初始位姿。
[0158] 在步骤S1720和步骤S1840中创建关于移动机器人的位姿发生变化后的周围环境 的局部地图的方法，可以采用与步骤S120中创建局部地图的相同的方法。在移动机器人的 位姿发生改变并且在步骤S1710或者S1840中、利用惯性导航传感器计算移动机器人在全局 地图中的第一位姿之后，可以设置移动机器人的假定位姿为该第一位姿，开启激光SLAM建 图功能，使得移动机器人在原位置自行旋转一周，形成较大的、详细的、360度的局部地图。 应当理解，要创建360度的局部地图，需要的移动机器人旋转的角度和移动机器人上布置的 激光扫描仪数量以及激光扫描仪视野范围有关。
[0159] 在步骤S1730和步骤S1850中将局部地图在全局地图中进行图像匹配，获取移动机 器人在全局地图中的第二位姿、和与第二位姿对应的匹配可信度的方法，可以采用与步骤 S130中进行局部地图和全局地图匹配的相类似的方法，从而可以获得在激光SLAM技术下的 关于惯性导航传感器计算的第一位姿的第二位姿表示、以及匹配可信度(或称为匹配相似 度)。
[0160] 在用于进行图像匹配的一个实施例中，依据当前惯性导航测量元件定义最大误差 范围，在根据惯性导航测量的第一位姿(xl，yl，91)最大误差范围内，可以用当前激光扫描 的局部地图数据与全局栅格地图基于蒙特卡洛方法进行图像匹配，即采用粒子滤波获取最 佳位姿、即第二位姿为(x2，y2，02)。附加地，还可以计算最佳匹配的匹配可信度。
[0161] 关于匹配可信度的计算，在获取最佳位姿后，移动机器人可以获得激光扫描范围 (在一定角度范围内按一定频率进行扫描）内的激光扫描距离信息，并且将获得的激光扫描 距离信息在原有全局栅格地图上进行映射来计算匹配可信度。
[0162] 移动机器人在导航时有两个坐标系，一是机器人坐标系（即以激光器为坐标原点， 激光器前后方向为Y轴，左右方向为X轴）；二是已建立好的全局地图坐标系，即所谓的世界 坐标系。全局地图表现的是实际所在环境所有障碍位置的表示，假设在激光观察点在全局 地图中（x，y)坐标位置，并且航向角为9。则从此处向9角度观察可以获取扫描范围内的障碍 物距离信息Dl(直接可以从全局地图中获得）。移动机器人配备的激光雷达可以在现实环境 中获得扫描范围内的障碍物距离信息D2A1和D2两个扫描距离信息越相近，则便是可信度 越高。这便是激光扫描距离信息与原始地图结合可获得可信度的原理。
[0163] 实际操作时如下，利用激光雷达在现实中获取的距离信息，映射到全局地图中表 示实际障碍所在的坐标，然后与原来全局地图中障碍物做比较，具体做法如下。
[0164] 在匹配可信度计算时，例如通过惯性导航方式可以获得当前位置和姿态(XC，yc，0 c)，其并不精确，表明当前准确的位姿就在其附近。假设获得当前的位姿(XC，yC，9 C)，在此 位姿激光雷达可扫描出N个距离(障碍)信息，转换成直角坐标且在机器人坐标系(激光器为 坐标原点(〇,〇)，航向角为〇)中可以看到激光扫描出的障碍轮廓。而机器人实际在全局地图 中的位姿为(xd，yd，0d)，依据映射原理可以算出激光扫描出的障碍轮廓在全局地图中的坐 标，即N个坐标。N是激光雷达在一定角度范围内按一定频率进行扫描（即在激光扫描范围内 进行扫描)获得的对于障碍物在地图中的像素数量。
[0165] 在一个实施例中，匹配可信度可以根据激光扫描获得的障碍轮廓在全局地图中的 映射轮廓(轮廓形状匹配)的坐标的像素和并且归一化来计算。匹配可信度可以选择全局地 图中这 N个坐标的像素和并归一化。归一化原因是确保每次获得的可信度范围为一定值，由 于激光扫描距离范围限制，假设激光扫描可以扫描点数为180个，但由于距离限制实际上测 量到有效距离信息的个数小于或等于180个。因此所有有效扫描点像素和不能直接作为可 信度，必须除以有效点个数，则可保证可信度dis_temp-定在0~255间。
[0166] 进一步可选地，由于在二维栅格地图中，障碍像素值为0，故di s_temp越小表明第 二位姿越精确。为适应匹配可信度意义，匹配可信度reliability可以为:reliability = 255-dis_temp。故匹配可信度越大，表明位姿越精确。激光导航测量原理结构图如图19所 不。
[0167] 参考图20,其图示了根据本发明的一个实施方式的移动机器人位姿确定方法2000 的流程图。方法2000在步骤S2060之前的步骤与方法1800中的对应步骤相同。方法2000进一 步包括步骤S2070-S2078，用于根据匹配可信度，确定移动机器人的位姿应当为惯性导航计 算的第一位姿，还是激光导航计算的第二位姿。
[0168] 在图20所示的实施方式中，匹配可信度越大，表明位姿越精确。所示意的激光导航 和惯性导航选择策略为：当激光导航策略中匹配可信度高于一定阈值时，表明激光导航确 定的第二位姿有效，移动机器人的位姿为第二位姿;否则激光导航确定的第一位姿无效，移 动机器人的位姿为惯性导航确定的第一位姿。如此，可消除由于外界环境临时变动而导致 激光导航策略获得的错误绝对位姿。
[0169] 在图20所示的实施方式中还能够实现惯性导航的累计误差消除，其原理可以为： 通过每时刻激光导航策略均对惯性导航策略的位姿进行修正，故避免了惯性导航累计误差 的出现;然而，当环境临时变动时，可导致当前位姿一定时间内连续使用惯性导航数据，即 使环境恢复，由于累计误差原因，使得当前位姿与实际位姿存在误差，可导致激光导航策略 中匹配可信度永远不满足阈值，从而仅有惯性导航。故当检测到激光导航可信度连续不满 足阈值一定时，需要重新进行初始位姿定位，以此循环。
[0170] 在进一步的实施方式中，可以进一步确定移动机器人惯性导航技术计算的第一位 姿的位姿误差，并且对该位姿误差进行误差校正。
[0171] 应当理解，附图中所示流程图图示了按照本发明各种实施例的方法可能实现的功 能和操作，在有些作为替换的实现中，方法流程的执行顺序可以以不同于附图中所标注的 顺序发生。
[0172] 在另一个方面，应当理解，本发明实施方式所提出的各种方法可以被实现为软件、 硬件或者软件和软件的组合。硬件可通过被设计来执行上述功能的转用集成电路(ASIC)、 数字信号处理器(DSP)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、 微处理器、其他电子单元或其组合来实现。软件可通过执行上述功能的模块来实现。软件可 被存储在存储单元中并由处理器运行。作为存储单元或处理器，可采用本领域技术人员公 知的各种单元。因此，本公开内容也意图涵盖能够实现所有这些方法的功能模块架构的装 置，这些装置可以被依软件、硬件、或者软件和硬件的结合来实现。
[0173] 举例而言，本发明实施方式提供了一种移动机器人的室内定位设备。所述移动机 器人包括激光扫描仪和惯性导航传感器。该设备可以包括:第一位姿确定装置，用于在所述 移动机器人的位姿发生改变时，利用所述惯性导航传感器计算所述移动机器人在全局地图 中的第一位姿；局部地图创建装置，用于使用所述激光扫描仪，利用即时定位与地图构建 (SLAM)技术创建关于所述移动机器人的周围环境的局部地图；第二位姿确定装置，用于将 所述局部地图在全局地图中进行图像匹配，获取所述移动机器人在所述全局地图中的第二 位姿、和与所述第二位姿对应的匹配可信度，其中所述第二位姿为图形匹配得出的所述移 动机器人在所述全局地图中的最佳位姿，并且其中所述全局地图是使用所述激光扫描仪， 利用即时定位与地图构建(SLAM)技术创建的关于所述移动机器人在其中导航的环境的地 图，以及最终位姿确定装置，用于根据所述匹配可信度，确定所述移动机器人的位姿为所述 第一位姿或者所述第二位姿。
[0174] 进一步地，所述设备还可以包括:估计装置，用于根据移动机器人室内定位技术估 计移动机器人的当前位姿;访问装置，用于访问参考物数据库，所述参考物数据库包括所述 移动机器人配备的摄像头拍摄的、关于至少一个参考物中的每个参考物的一幅参考图像， 以及与所述参考图像关联的、所述摄像头拍摄所述参考图像时所述移动机器人的相应真实 位姿，并且从所述参考物数据库中选择一个真实位姿和相关联的参考图像，其中所述参考 物是室内环境中的静止物体;导航装置，用于将所述移动机器人导航至目标位姿，所述目标 位姿的数值是根据所述移动机器人室内定位技术对于所选择的真实位姿的估计值;拍摄装 置，用于利用所述移动机器人配备的所述摄像头进行图像拍摄，获得拍摄图像;对比装置， 用于将所述拍摄图像中包括的参考物图像、即实际参考物图像与所选择的相关联的参考图 像中包括的参考物图像、即数据库参考物图像进行对比；以及确定装置，用于根据对比的结 果来确定所述移动机器人室内定位技术产生的位姿误差。
[0175] 进一步地，所述确定装置可以进一步包括:调整和特征匹配装置，用于根据对比的 结果调整所述移动机器人的位姿，并且对所述移动机器人配备的所述摄像头所拍摄的拍摄 图像中包括的实际参考物图像与所述数据库参考物图像进行特征匹配，使得达到最佳匹配 度；以及位姿误差确定装置，用于根据特征匹配的结果来确定所述移动机器人室内定位技 术产生的位姿误差。
[0176] 进一步地，所述调整和特征匹配装置可以进一步包括:第一调整和特征匹配装置， 用于通过对移动机器人进行以下各项操作中的一项或多项：向左和/或向右移动、向左和/ 或向右旋转、以及向前和/或向后调整，使得所述移动机器人配备的所述摄像头所拍摄的拍 摄图像中包括的实际参考物图像与所述数据库参考物图像在形状方面和尺寸方面达到最 佳匹配度;第二调整和特征匹配装置，用于通过向左和/或向右旋转移动机器人，使得所述 摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像在形状方面 达到最佳匹配度；或者第三调整和特征匹配装置，用于通过向前和/或向后调整移动机器 人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像 在尺寸方面达到最佳匹配度。
[0177] 进一步地，所述参考物数据库还可以包括与所述移动机器人的一个真实位姿相关 联的多幅参考图像，所述多幅参考图像包括所述移动机器人配备的摄像头在所述移动机器 人的如下位姿拍摄的关于参考物的参考图像集合、以及拍摄每幅参考图像时的所述移动机 器人的相应真实位姿:所述移动机器人的所述真实位姿，在所述移动机器人的真实位姿处 向左和向右各旋转n次所述移动机器人的最小旋转角度，在所述移动机器人的真实位姿处 向左和向右各移动m次所述移动机器人的最小移动距离，以及在所述移动机器人的真实位 姿处向左和向右各移动m次所述移动机器人的最小移动距离并且在每个移动后的位置处向 左和向右各旋转n次所述移动机器人的最小旋转角度，其中m和n为自然数。所述调整和特征 匹配装置可以进一步包括:第四调整和特征匹配装置，用于通过向左和/或向右旋转移动机 器人，使得所述摄像头所拍摄的拍摄图像中包括的实际参考物图像与所述多幅参考图像中 包括的数据库参考物图像在形状方面达到最佳匹配度；以及第五调整和特征匹配装置，用 于通过向前和/或向后调整移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实际 参考物图像与所述数据库参考物图像在尺寸方面达到最佳匹配度。
[0178] 进一步地，所述确定装置可以进一步包括:第一确定装置，用于根据所述移动机器 人室内定位技术估计特征匹配后的所述移动机器人的位姿(P1，A1)，并且根据所述特征匹 配后的位姿(P1，A1)和所述参考物数据库中存储的、与拍摄所述达到最佳匹配度的参考图 像时的所述移动机器人的相应真实位姿来确定所述移动机器人室内定位技术产生的位姿 误差。
[0179] 进一步地，所述确定装置可以进一步包括:第二确定装置，用于通过所述实际参考 物图像和所述数据库参考物图像距离各自拍摄图像左边缘或者右边缘的距离差来确定航 向误差;或者第三确定装置，用于通过移动机器人拍摄所述实际参考物图像时距离参考物 的距离、和参考物数据库中存储的移动机器人拍摄数据库参考物图像时距离参考物的距离 来确位姿误差。
[0180] 本发明实施方式的教导还可以实现为一种可以在计算机可读存储介质中实现的 计算机程序产品，所述计算机可读存储介质具有存储于其中的计算机可读程序代码部分， 所述计算机可读程序代码部分被配置为执行如前所述的根据本发明实施方式的方法或者 所述方法的各个步骤。
[0181] 根据本发明进一步实施方式的用于移动机器人室内定位的方案，包括前述方法、 设备和计算机程序产品，可以及时发现移动机器人在室内导航过程中所产生的位姿误差， 并且进一步地，在必要时可以对该位姿误差进行校正。
[0182] 应当理解，本发明的实施方式的方法或设备可以被以软件、硬件、或者软件和硬件 的结合来实现。尽管在上文的详细描述中提及了系统的若干装置或子装置，但是这种划分 仅仅是示例性而非强制性的。实际上，根据本发明的示例性实施方式，上文描述的两个或更 多装置的特征和功能可以在一个装置中实现。反之，上文描述的一个装置的特征和功能可 以进一步划分为由多个装置来实现。
[0183]以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精 神和原则之内，所作的任何修改、等同替换等，均应包含在本发明的保护范围之内。
【主权项】
1. 一种移动机器人的位姿确定方法，所述移动机器人包括激光扫描仪，其特征在于，所 述方法包括： 使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建所述移动机器人在其 中导航的环境的全局地图； 在所述移动机器人上电时，使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技 术创建针对所述移动机器人的上电时刻的环境的局部地图；以及 将所述局部地图在所述全局地图中进行图像匹配，获取所述移动机器人在全局地图中 的初始位姿。2. 根据权利要求1所述的方法，其特征在于，所述移动机器人还包括惯性导航传感器， 所述方法还包括： 在所述移动机器人的位姿发生改变时，利用所述惯性导航传感器计算所述移动机器人 在所述全局地图中的第一位姿； 使用所述激光扫描仪，利用即时定位与地图构建(SLAM)技术创建关于所述移动机器人 的周围环境的局部地图； 将所述局部地图在全局地图中进行图像匹配，获取所述移动机器人在所述全局地图中 的第二位姿和与所述第二位姿对应的匹配可信度，其中所述第二位姿为图形匹配得出的所 述移动机器人在所述全局地图中的最佳位姿；以及 根据所述匹配可信度，确定所述移动机器人的位姿为所述第一位姿或者所述第二位 姿。3. 根据权利要求2所述的方法，其特征在于，所述匹配可信度为使用所述激光扫描仪在 所述局部地图中进行激光扫描获得的障碍轮廓在所述全局地图中的映射轮廓的坐标的像 素和、并且对所述像素和进行归一化的结果。4. 根据权利要求1所述的方法，其特征在于，还包括： 在移动机器人位姿初始化或者位姿发生改变时，根据移动机器人室内定位技术估计移 动机器人的当前位姿； 访问参考物数据库，所述参考物数据库包括所述移动机器人配备的摄像头拍摄的、关 于至少一个参考物中的每个参考物的一幅参考图像，以及与所述参考图像关联的、所述摄 像头拍摄所述参考图像时所述移动机器人的相应真实位姿，并且从所述参考物数据库中选 择一个真实位姿和相关联的参考图像，其中所述参考物是室内环境中的静止物体； 将所述移动机器人导航至目标位姿，所述目标位姿的数值是根据移动机器人室内定位 技术对于所选择的真实位姿的估计值； 利用所述移动机器人配备的所述摄像头进行图像拍摄，获得拍摄图像； 将所述拍摄图像中包括的参考物图像、即实际参考物图像与所选择的相关联的参考图 像中包括的参考物图像、即数据库参考物图像进行对比； 根据对比的结果来确定所述移动机器人室内定位技术产生的位姿误差；以及 利用所确定的位姿误差对根据所述移动机器人室内定位技术所估计的移动机器人的 位姿进行误差校正。5. 根据权利要求4所述的方法，其特征在于，根据对比的结果来确定所述移动机器人室 内定位技术产生的位姿误差进一步包括： 根据对比的结果调整所述移动机器人的位姿，并且对所述移动机器人配备的所述摄像 头所拍摄的拍摄图像中包括的实际参考物图像与所述数据库参考物图像进行特征匹配，使 得达到最佳匹配度；以及 根据特征匹配的结果来确定所述移动机器人室内定位技术产生的位姿误差。6. 根据权利要求5所述的方法，其特征在于，所述根据对比的结果调整所述移动机器人 的位姿，并且对所述移动机器人配备的所述摄像头所拍摄的拍摄图像中包括的实际参考物 图像与所述数据库参考物图像进行特征匹配，使得达到最佳匹配度包括： 通过对移动机器人进行以下各项操作中的一项或多项：向左和/或向右移动、向左和/ 或向右旋转、以及向前和/或向后调整，使得所述移动机器人配备的所述摄像头所拍摄的拍 摄图像中包括的实际参考物图像与所述数据库参考物图像在形状方面和尺寸方面达到最 佳匹配度； 通过向左和/或向右旋转移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实 际参考物图像与所述数据库参考物图像在形状方面达到最佳匹配度;或者 通过向前和/或向后调整移动机器人，使得所述摄像头所拍摄的拍摄图像中包括的实 际参考物图像与所述数据库参考物图像在尺寸方面达到最佳匹配度。7. 根据权利要求1-6中任一项所述的方法，其特征在于，所述根据特征匹配的结果来确 定所述移动机器人室内技术产生的位姿误差包括： 根据所述移动机器人室内定位技术估计特征匹配后的所述移动机器人的位姿（Pl， Al)，并且根据所述特征匹配后的位姿(PI，A1)和所述参考物数据库中存储的、与拍摄所述 达到最佳匹配度的参考图像时的所述移动机器人的相应真实位姿来确定所述移动机器人 室内定位技术产生的位姿误差。8. 根据权利要求4所述的方法，其特征在于，根据对比的结果来确定所述移动机器人室 内定位技术产生的位姿误差进一步包括： 通过所述实际参考物图像和所述数据库参考物图像距离各自拍摄图像左边缘或者右 边缘的距离差来确定航向误差;或者 通过移动机器人拍摄所述实际参考物图像时距离参考物的距离和参考物数据库中存 储的移动机器人拍摄数据库参考物图像时距离参考物的距离来确位姿误差。9. 一种移动机器人的位姿确定设备，所述移动机器人包括激光扫描仪，其特征在于，所 述设备包括： 全局地图创建装置，用于使用所述激光扫描仪，利用即时定位与地图构建（SLAM)技术 创建所述移动机器人在其中导航的环境的全局地图； 局部地图创建装置，用于在所述移动机器人上电时，使用所述激光扫描仪，利用即时定 位与地图构建(SLAM)技术创建针对所述移动机器人的上电时刻的环境的局部地图；以及 初始位姿获取装置，用于将所述局部地图在所述全局地图中进行图像匹配，获取所述 移动机器人在全局地图中的初始位姿。10. -种在计算机可读存储介质中实现的计算机程序产品，所述计算机可读存储介质 具有存储于其中的计算机可读程序代码部分，所述计算机可读程序代码部分被配置为执行 根据权利要求1-8所述的方法。
【文档编号】G01C11/06GK105928505SQ201610244325
【公开日】2016年9月7日
【申请日】2016年4月19日
【发明人】王加加, 刘彪, 王可可, 刘英英
【申请人】深圳市神州云海智能科技有限公司