基于红外路标的移动机器人定位系统和方法

摘要

本发明公开了一种基于红外路标的移动机器人室内定位系统。系统结构在于：采用红外发射二极管制作点阵路标，并贴附于室内天花板上；广角红外摄像机固定在移动机器人身上，向上拍摄红外路标；通过机器人身上的计算机进行图像分析、实时计算出机器人的位姿。本发明所述定位系统可以用于移动机器人在较大范围的室内场所的定位，且计算速度快、定位精度高、抗干扰性强。

权利要求

1.一种基于红外路标的移动机器人室内定位系统，该系统包括：

点阵式主动红外路标，贴附于室内天花板上，为m×m型点阵式，红外路标的世界坐标值被预先确定，其中m是大于等于3的整数；

广角红外摄像机，固定在移动机器人身上，用于捕捉红外图像，广角摄像机的内参和畸变参数被预先标定，其初始单应矩阵H0也被预先确定；

计算机单元，用于从广角红外摄像机接收所述红外图像，并对红外图像进行畸变校正及图像预处理操作，并根据处理后的红外图像来检测和识别红外路标，然后利用初始单应矩阵H0，采用扩展单应矩阵定位对移动机器人进行定位计算，其中世界坐标XY平面建立在路标平面上，摄像机运动平面与路标平面平行，摄像机的运动包括平行移动以及以地面的垂线为旋转轴做旋转运动，扩展单应矩阵定位进一步包括：利用运动的相对性，将固定在机器人身上的摄像机相对于路标平面的旋转平移运动，转化为路标相对于摄像机坐标系的旋转平移运动；通过假设单应矩阵不变来计算出虚拟路标的位置，由虚拟路标与实际路标之间的位姿差异推算出摄像机的位姿变化，从而达到移动机器人定位的目的。

2.根据权利要求1所述的定位系统，其特征在于，所述点阵式主动红外路标由贴片式红外发射二极管制作，近红外波长为850nm。

3.根据权利要求2所述的定位系统，其特征在于，所述点阵式主动红外路标的点阵为

3×3型，由外围较大的四点和其余较小的点组成，较大的四点用于路标的检测与定位计算，其余较小的点用于路标的ID值计算。

4.根据权利要求3所述的定位系统，其特征在于，所述红外广角摄像机采用焦距为

2.8mm或者2.5mm的大广角镜头，镜头前方安装中心波长为850nm的窄带通滤波片。

5.根据权利要求4所述的系统，其特征在于，所述计算机单元通过点域检测、路标点集聚类、路标选择、四个定位点的辨识以及ID值计算来检测和识别路标。

6.一种基于红外路标的移动机器人室内定位方法，该方法包括步骤：

将3×3型的点阵式主动红外路标帖附于天花板上，确定所有红外路标的世界坐标值；

将广角红外摄像机固定在机器人身上，标定广角摄像机的内参和畸变参数，计算摄像机的初始单应矩阵H0；

确定移动机器人的初始位姿；

捕捉红外图像，并进行畸变校正及图像预处理操作；

检测并识别红外路标；

利用初始单应矩阵H0，采用扩展单应矩阵定位进行定位计算，其中世界坐标XY平面建立在路标平面上，摄像机运动平面与路标平面平行，摄像机的运动包括平行移动以及以地面的垂线为旋转轴做旋转运动，扩展单应矩阵定位进一步包括：利用运动的相对性，将固定在机器人身上的摄像机相对于路标平面的旋转平移运动，转化为路标相对于摄像机坐标系的旋转平移运动；通过假设单应矩阵不变来计算出虚拟路标的位置，由虚拟路标与实际路标之间的位姿差异推算出摄像机的位姿变化，从而达到移动机器人定位的目的。

7.根据权利要求6所述的定位方法，其特征在于，所述点阵式主动红外路标由贴片式红外发射二极管制作，近红外波长为850nm。

8.根据权利要求7所述的定位方法，其特征在于，所述点阵式主动红外路标的点阵由外围较大的四点和其余较小的点组成，较大的四点用于路标的检测与定位计算，其余较小的点用于路标的ID值计算。

9.根据权利要求8所述的定位方法，其特征在于，所述红外广角摄像机采用焦距为

2.8mm或者2.5mm的大广角镜头，镜头前方安装中心波长为850nm的窄带通滤波片。

10.根据权利要求9所述的定位方法，其特征在于，所述计算机单元通过点域检测、路标点集聚类、路标选择、四个定位点的辨识以及ID值计算来检测和识别路标。

说明书

基于红外路标的移动机器人定位系统和方法

技术领域

[0001] 本发明涉及红外成像技术领域，具体地涉及移动机器人视觉定位技术领域。

背景技术

[0002] 随着计算机技术、超大规模集成电路技术、网络技术、人工智能技术等的飞速发展，机器人技术也得到了突飞猛进的发展。机器人的种类越来越多，应用范围也越来越广。2007年，微软总裁比尔·盖茨在《科学美国人》杂志中预言：机器人将与30年前的个人电脑一样进入千家万户。韩国三星经济研究所曾经预测，到2020年，世界机器人市场规模到将达到1.4万亿美元，韩国信息与通信部甚至曾订出2020年每家都有一个机器人的惊人目标。对于智能移动机器人来说，为了能高效地从环境中穿行并到达目的地，并使它能根据环境中己知的一些特征来判断出它自身的位置，这就是移动机器人的定位问题。机器人要想能在未知环境中自动行走，定位是最基本的问题。

[0003] 近年来国内外许多研究人员使用多种传感器，对移动机器人的自定位问题进行了深入研究，提出了许多自定位方法。自定位方法包括：拓扑表示法、航位推算法、卡尔曼滤波估计、栅格法、概率法、同时定位与制图法等。用于定位的传感器包括：视觉传感器、激光、红外、超声、码盘、陀螺仪、加速度计等。码盘、陀螺仪、加速度计是用于局部定位的辅助传感器。红外、超声传感器受精度所限，一般用于紧急避障。激光传感器成本较高，不适合民用推广。视觉传感器获取的环境信息最为丰富，发展空间最大。目前研究人员都侧重于机器人的视觉定位研究。

[0004] 移动机器人的视觉定位一般分自然路标和人工路标两种模式。自然路标是指利用环境中原有的场景作为标记进行定位导航。虽然自然路标不破坏原有环境，普适性好，但计算复杂、鲁棒性不强、实用性差。人工路标是指人为设计特定的路标安装在环境中，人工路标虽然对环境有所改变，但计算简单、特征稳定、实用性强。目前研究人员所采用的人工路标多是设计特殊颜色信息或者纹理结构信息的图案，也包括数字、字母以及二维条码等。这些图案一般都是用纸片打印制作，通过环境光照明使摄像机感光成像，因此容易受环境光照变化的影响，从而稳定性较差。

发明内容

[0005] 本发明的目的是为避免上述现有技术所存在的不足之处，提供一种基于红外路标的移动机器人视觉定位系统和方法，实现针对移动机器人的准确、快速、鲁棒的定位。

[0006] 本发明的基于红外路标的移动机器人室内定位系统包括：点阵式主动红外路标，贴附于室内天花板上，为3×3型点阵式，红外路标的世界坐标值被预先确定；广角红外摄像机，固定在移动机器人身上，用于捕捉红外图像，广角摄像机的内参和畸变参数被预先标定，其初始单应矩阵H0也被预先确定；计算机单元，用于从广角红外摄像机接收所述红外图像，并对红外图像进行畸变校正及图像预处理操作，并根据处理后的红外图像来检测和识别红外路标，然后利用初始单应矩阵H0，采用扩展单应矩阵定位方法对移动机器人进行定位计算。

[0007] 本发明还提供了一种基于红外路标的移动机器人室内定位方法，该方法包括步骤：将3×3型的点阵式主动红外路标帖附于天花板上，确定所有红外路标的世界坐标值；将广角红外摄像机固定在机器人身上，标定广角摄像机的内参和畸变参数，计算摄像机的初始单应矩阵H0；确定移动机器人的初始位姿；捕捉红外图像，并进行畸变校正及图像预处理操作；检测并识别红外路标；利用初始单应矩阵H0，采用扩展单应矩阵定位方法进行定位计算。

[0008] 本发明的优点如下：本发明提出的红外路标模式制作简单，方便图像检测、识别以及定位计算；定位算法原理简单、计算快速、定位准确。本发明提出的定位系统可用于较大范围的室内场所进行移动机器人的快速、准确、鲁棒的定位。

附图说明

[0009] 图1为本发明的系统组成示意图；

[0010] 图2为本发明中的红外路标示意图；

[0011] 图3为本发明的定位系统的离线工作流程图；

[0012] 图4为本发明的定位系统的在线工作流程图；

[0013] 图5为真实mark与虚拟mark之间的平移旋转关系图。

具体实施方式

[0014] 为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

[0015] 图1给出了本发明的基于红外路标的移动机器人定位系统的示意图。

[0016] 参照图1，该系统包括：贴附于室内天花板上的、3×3型的点阵式主动红外路标1；固定在机器人身上的广角红外摄像机2；用于图像分析与定位计算的计算机单元3。

[0017] 天花板平面与地面平行，红外路标1贴附于天花板平面，广角红外摄像机2固定在机器人平台上方，向上拍摄红外路标，用于图像分析与定位计算的计算机单元3安置在移动机器人平台内部。红外路标1的安装距离需要结合摄像机的视野大小以及摄像机离天花板的高度综合考虑，以使得摄像机不存在定位死角为准，即摄像机在场景中任何地方至少能拍摄到一个路标，在天花板平面上贴有多个红外路标1。

[0018] 红外路标1的点阵由贴片式红外发射二极管发光实现。红外发射二极管的发光波长一般有850nm和940nm两种，由于CCD对850nm光波的感光度大于940nm的光波，因此选用波长为850nm的红外发射二极管。红外路标1的具体设计模式如图2所示。3*3的点阵中有两种点集，外围面积较大的四点和其余小点。每个路标都有位置相同的四个大点，用于确定路标的子坐标系，即确定出路标点阵的原点和XY坐标轴，另外，该四点在定位算法中用于计算单应矩阵。剩下的5个小点用于计算路标的ID值以区分每个路标，组合数为32，即可以表示32种不同的路标。ID值的计算采用二进制编码的方式，图2中给出了每个小点的二进制码值，ID值范围为0-31。

[0019] 为了使单个路标覆盖的定位范围尽可能的广、使用的路标数量尽可能的少，所用摄像机必须有足够大的视野范围，因此红外广角摄像机采用焦距为2.8mm或者2.5mm的大广角镜头。镜头前方安装中心波长为850nm的窄带通滤波片，从而只对850nm(路标点阵红外二极管对应的光波波长)的光波进行感光成像。

[0020] 本发明的基于红外路标的移动机器人室内定位系统的工作过程包括离线过程和在线过程。其中离线过程涉及定位环境的配置和各种参数的计算。

[0021] 参照图3，离线过程包括：标定广角摄像机的内参和畸变参数；确定所有红外路标的世界坐标值；确定移动机器人的初始位姿；计算摄像机的初始单应矩阵H0。

[0022] 参照图3，离线过程具体包括以下步骤：

[0023] 步骤S301，先完成摄像机标定工作，标定广角摄像机的内参和畸变参数，将摄像机内参以及畸变校正的映射矩阵存于相应文档T1。

[0024] 步骤S302，制作路标，具体地，采用条形电路板搭成“田”字型架子，在交叉点处焊接贴片红外发射二极管。

[0025] 步骤S303，将路标贴附于天花板平面，将摄像机安装在移动机器人平台上方。

[0026] 定位环境搭建好后，剩下的工作就是建立地图信息以及测定用于定位计算的参数，地图信息主要是记录各个路标的子坐标系在世界坐标中的位置，通过步骤S304计算出每个路标的世界坐标值，并存入相应文档T2。每个路标的世界坐标值确定方法如下：由于每个路标上都有四个定位点，所以可以利用单应矩阵关系将路标的位置一一确定。路标的世界坐标数据存储在一个文件中，格式为：

[0027] 0：1，0 460，0 0，460 0，460 230

[0028] 1：0，0 0，0 0，0 0，0 0

[0029] 2：0，0 0，0 0，0 0，0 0

[0030] … … …

[0031] … … …

[0032] … … …

[0033] 31：0，0 0，0 0，0 0，0 0 (1)

[0034] 每行数据的格式说明如下：

[0035] 路标ID：该路标世界坐标是否确定(是1，否0)，定位点0的世界坐标值(每个坐标之间以空格分隔)，定位点1的世界坐标值(每个坐标之间以空格分隔)，定位点2的世界坐标值(每个坐标之间以空格分隔)，定位点3的世界坐标值(每个坐标之间以空格分隔)。

[0036] 假定选ID为0的路标作为原点路标，即以0号路标的子坐标系作为世界坐标。其它路标的世界坐标计算的过程如下：1.检测出摄像视野中的所有路标，并识别出它们的ID值。2.从路标世界坐标文档中搜索识别出来的路标，如果有某个路标的世界坐标已经确定(通过标识判断)，则利用该路标的四个定位点计算出当前图像的单应矩阵H。利用H计算出其他未确定世界坐标值的路标的四个定位点坐标，然后将数据存入文档。3.如果视野中的路标都没有确定，则移动机器人，重新开始第一步。4.当能拍摄到的路标都已经确定完后，计算结束。

[0037] 接下来，在步骤S305计算初始单应矩阵H0，并将计算结果存入H0文档T3。初始单应矩阵的确定方法说明如下：本发明采用的基于平面假设的扩展单应矩阵定位方法中，以摄像机坐标系为参照，即单应矩阵不发生变化，路标产生虚拟位移。后面的定位计算都是针对初始单应矩阵进行的。由于所有路标都统一到世界坐标系下，所以初始单应矩阵可以任选一个路标中的四个点进行计算。

[0038] 单应矩阵的确定方法如下：

[0039] 路标中各点的位置与摄像机3成像平面构成的投影关系为平面到平面的射影关系，即单应矩阵变换关系，公式表述如式2所示。

[0040]

[0041] 上式中，H为单应矩阵，mi(i＝1，2，...，9)为单应矩阵的矩阵元素，si为比例系数，(ui，vi)为路标点在图像中的坐标，(xi，yi)为路标点的世界坐标。将式(2)展开为三个方程，

[0042]

[0043] 将第一个方程除以第三个方程，第二个方程除以第三个方程，消掉si得到两个方程：

[0044]

[0045] 如果已知物体平面上的n个点，知道它们的空间坐标(xi，yi)(i＝1，2，...，n)，并且还已知它们的图像点坐标(ui，vi)(i＝1，2，...，n)，则我们有2n个关于单应矩阵元素的线性方程，下面用矩阵形式写出这些方程：

[0046]

[0047] 可以令m9＝1，令方程系数为矩阵K，变量为M，值为U，则方程可写为KM＝U，方程的最小二乘解为：

[0048] M＝(KTK)-1KTU (6)

[0049] 存在8个未知数，可通过四对点求解方程，从而得到单应矩阵的各个矩阵元素值。

[0050] 最后完成步骤S306，计算出摄像机光心的初始位置(x0，y0)，并存入文档T4。摄像机光心初始位置的确定方法说明如下：摄像机内参事先已经标定好，摄像机外参是目标点从世界坐标系转化到摄像机坐标系的转化矩阵，包括三个旋转向量和一个平移向量。用公式表达为：

[0051]

[0052] (Xc，Yc，Zc)为目标点在摄像机坐标系下的坐标值，(Xw，Yw，Zw)为目标点在世界坐标系下的坐标值。M2为摄像机外参矩阵，r1、r2、r3为摄像机外参的三个正交的旋转向量，t为摄像机外参的平移向量。选用ID为31的红外路标作为原点路标，在路标下选择任意位姿计算单应矩阵H0，同时利用路标上9对共面点求解摄像机外参M2。摄像机光心的定位是要算出光心在世界坐标系下的坐标值。计算公式如下：

[0053] (Xc＝0，Yc＝0，Zc＝0)

[0054] (8)

[0055] 图4为本发明的基于红外路标的移动机器人室内定位系统的在线工作过程。在线工作的任务主要是利用离线工作测定的各种参数，检测并识别红外路标，采用扩展单应矩阵定位方法进行摄像机的定位计算。

[0056] 参照图4，在线过程包括：摄像机2捕捉天花板上的红外图像，并将捕获的图像传送到计算机单元3；计算机单元3对接收的图像进行畸变校正及图像预处理操作；计算机单元3检测并识别红外路标；计算机单元3利用初始单应矩阵H0，采用扩展单应矩阵定位方法进行定位计算。

[0057] 参照图4，离线过程具体包括以下步骤：

[0058] 首先，在步骤S401，通过设置在机器人上的摄像机获取天花板上的路标图像后，利用畸变校正矩阵T1进行图像的预处理工作，包括图像校正、图像滤波、二值化、形态学处理等。

[0059] 在步骤S402，检测出视野中的所有路标(由二极管组成的“田”字路标)并选择位于图像中间的的路标作为原点路标。

[0060] 在步骤S403，针对所选择的路标进行四个外围定位点的检测。

[0061] 在步骤S404，计算路标ID值。

[0062] 在步骤S405，利用离线测得的路标世界坐标值T2、初始单应矩阵T3以及摄像机光心的初始位置T3，采用扩展单应矩阵定位方法进行定位计算。

[0063] 扩展单应矩阵定位方法的前提是平面假设：1.路标贴附于天花板平面，摄像机固定于移动平台上方，世界坐标XY平面建立在路标平面上；2.摄像机运动平面与路标平面平行；3.摄像机的运动只有两种，平行移动，以地面的垂线为旋转轴做旋转运动。扩展单应矩阵定位方法的原理为：在平面假设的前提下，利用运动的相对性，将固定在机器人平台上的摄像机相对于路标平面的旋转平移运动，转化为路标相对于摄像机坐标系的旋转平移运动；通过假设单应矩阵不变来计算出虚拟路标的位置，由虚拟路标与实际路标之间的位姿差异推算出摄像机的位姿变化，从而达到移动机器人定位的目的。摄像机的运动前后两个位姿之间的关系可以用式(9)表示，而对应的以摄像机为参照的标记点的运动位姿关系可以用式(10)表示。

[0064]

[0065]

[0066] 式(9)中，PCam、P′Cam、P″Cam为摄像机的实际位姿，式(10)中，PMark、P′Mark、P″Mark为路标的虚拟位姿。

[0067] 结合离线过程中确定的参数，定位计算过程包括：

[0068] 首先，计算路标虚拟位姿P″Mark。

[0069] 在平面假设下的运动定位计算过程中，单应矩阵保持不变，将移动平台的运动等效于路标反方向的平移/旋转。假设路标反方向平移/旋转后的位姿为P″Mark。

[0070] 结合初始单应矩阵H0，可以推导出求解通过图像点坐标求解点坐标的公式：

[0071]

[0072]

[0073] 其中，mi为单应矩阵H0的元素，(xi，yi)为物点的世界坐标，(ui，vi)为图像坐标。当摄像机处于新的位姿时，检测出图像中的路标，即得到图像坐标(ui，vi)，然后结合H0利用式(11)算出路标的世界坐标(xi，yi)，由于计算中采用的单应矩阵是摄像机在初始位置时的单应矩阵，所以算出来的(xi，yi)是虚拟路标的世界坐标，三个路标点即可以构成虚拟路标位姿P″Mark。图5给出了原始路标及虚拟路标的关系图，p0 p1 p2三点为真实的路标点，p′0 p′1 p′2为只有平移运动时的虚拟路标点，p″0 p″1 p″2为平移加旋转后的虚拟路标点。o′为摄像机光心相对于世界坐标的初始位置。利用H0反算出来的P″Mark即对应p″0 p″1 p″2。

[0074] 其次，计算转角θ。

[0075] p0 p1 p2与p″0 p″1 p″2之间的角度差就是旋转角θ。θ可通过计算两矢量与 的角度差得出。将 归一化为(cos(θ1)，sin(θ1))， 归一化为(cos(θ2)，sin(θ2))，则有θ＝θ1-θ2。

[0076] 最后，需要计算位移S。

[0077] 上面已算出旋转角θ和p″0 p″1 p″2，只需将p″0 p″1 p″2以o′为中心逆时针旋转θ即得到p′0 p′1 p′2。式(12)为p″0旋转得到p′0的公式，其他两点类似。

[0078]

[0079] 理论上来讲三个位移量 应该相等，但由于存在计算误差，可以通过取平均的方式来得到摄像机的位移量S。

[0080]

[0081] 通过以上转角θ和位移S的计算，即确定了机器人相对于原点路标的位姿，达到了空间相对定位的目的。

[0082] 在步骤S406，对定位结果进行滤波处理，可采用卡尔曼滤波方法进行。

[0083] 上述实施例给出的是本发明所提出的基于红外路标的移动机器人定位系统的基本结构方案与算法操作流程，本发明的保护范围并不局限于此，还包括采用该结构方案以及定位算法而在外围稍作改动的其他方案，如点阵路标改为m*m(m是大于等于3的整数)的结构或者移动机器人平台改为移动车辆平台等。

[0084] 以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。