基于视觉的机器人全局定位

ｚ。１［Ｍ１口１ １］。＝Ｍ１［ｚ。Ｙ。ｚ。１］。
…
ｚｃ２［“２秽２ １］。＝鸩［戈。Ｙ。ｚ。１］Ｔ
其中ｚ。。与ｚ以分别表示Ｐ在摄像头坐标系ｃ。与ｃ：
下的彳坐标，Ｍ。与坞分别是ｔ，和ｔ：时刻的投影矩 阵．假设投影矩阵鸠与图像坐标已知，根据以上方
程，消除未知参量ｚ。。与ｚ。：可以获得４个方程，利用最 ｔＪ、－－乘法对４个方程进行求解，可以获得Ｐ的三维 坐标．ｔ时刻的投影矩阵Ｍ。为：
Ａｂｓｔｒａｃｔ：Ａｎ ｅｎｖｉｒｏｎｍｅｎｔａｌ ｍａｐ ｂｕｉｌｔ ｗｉｔｈ ｍｏｎｏｃｕｌａｒ ｖｉｓｉｏｎ ｉｓ ｕｓｅｄ ｔｏ ｉｍｐｌｅｍｅｎｔ ｉｎｏｂｉｌｅ ｒｏｂｏｔ ｇｌｏｂａｌ ｌｏｃａｌｉｚａｔｉｏｎ．Ｔｈｅ ｆｅａｔｕｒｅ ｍａｔｃｈｉｎｇ ｉｓ ｉｍｐｌｅｍｅｎｔｅｄ ｗｉｔｈ ｔｈｅ ＫＤ．ｔｒｅｅ．ｂａｓｅｄ ｎｅａｒｅｓｔ ｓｅａｒｃｈ ａｐｐｒｏａｃｈ．Ｔｈｅ ｆｅａｔｕｒｅｓ ａｒｅ ｅｘｔｒａｃｔｅｄ ｗｉｔｈ Ｓｃａｌｅ Ｉｎ—
表明本文提出方法高效、可靠．
关键词：移动机器入；全局定位；ＫＤ树；特征提取；ＲＡＮＳＡＣ；单目视觉
中图分类号：ＴＰ２４
文献标识码：Ａ
Ｍｏｎｏｃｕｌａｒ．。Ｖｉｓｉｏｎ．Ｂａｓｅｄ Ｍｏｂｉｌｅ Ｒｏｂｏｔ Ｇｌｏｂａｌ Ｌｏｃａｌｉｚａｔｉｏｎ
ＬＩ Ｍａｏ—ｈａｉｌ，ＨＯＮＧ Ｂｉｎｇ—ｒｏｎ９１，ＬＵＯ Ｒｏｎｇ—ｈｕａ２，ＣＡＩ Ｚｅ—ＳＵｌ （１．Ｓｃｈｏｏｌ ｏｆ Ｃｏｍｐｕｔｅｒ Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ，Ｈａｒｂｉｎ Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｔｅｃｈｎｏｌｏｇｙ，Ｈａｒｂｉｎ １５０００１，Ｃｈｉｎａ； ２．Ｓｃｈｏｏｌ ｏｆＣｏｍｐｕｔｅｒ Ｓｃｉｅｎｃｅ ａｎｄ Ｅｎｇｉｎｅｅｒｉｎｇ，Ｓｏｕｔｈ Ｃｈｉｎａ Ｕｎｉｖｅｒｓｉｔｙ ｏｆＴｅｃｈｎｏｌｏｇｙ，Ｃｕａｎｇｚｈｏｕ ５１０６４０，Ｃｈｉｎａ）
的两个数组分别为＾ｒ。ｉ。和ｈｒ。。，则Ｐ的第ｉ维元素
Ｐ¨’计算如下： ｒｈｒ２ｉ：
ｉｆ ｔａｒｇｅｔ¨’≤ｈｒ‘ｍｉｉ：
Ｐ¨’＝｛ｔａｒｇｅｚ（ｊ’ｉｆ危ｒ盆≤ｔａｒｇｅｔ“’≤ｈｒ２（２）
，
Ｌ矗ｒ盛
ｉｆ ｔａｒｇｅｔ“’≥ｈｒ墨
当ｌｐ｛ｊｔａｒｇｅｔ ｌ≤ｒ时，衍与以ｔａｒｇｅｔ为中心以ｒ
为半径的超球体相交．为了更进一步提高匹配的准
『ｏ ｆａｚ＾３］
Ｆ＝ｌ厶０厶Ｉ
（６）
ｂ。厶：１ ｊ
因此，只需要５个匹配对就可以求解Ｆ：
，曩沁Ｆｆ：，；’＝（秽≯五，＋六。）ｕ∥＋（“≯＾：＋六：）秽∥
｛
＋“÷¨＾３＋秽÷¨五３＋１＝０
ｏｌＦｌ＝＾：五，六。＋Ｙｌ，＾。兀２一＾：＾。＝０
（７）
其中ｉ＝１，…，５，（ｕ；“，秽∥，１）是当前ｔ时刻观测的
ｒｄ。０ ｕｏ］
托＝ｊ ０ ｄ，％ｆ［Ｒ。ｔ］
（４）
ｌ ｏ ０『 １ ｊ
其中Ｒ。与ｒ。分别是摄像头坐标系在世界坐标系下 的旋转矩阵与平移矢量；叱＝ｆ／ｄ戈，％＝ｆ／ｄｙ，表示焦
距，每一个像素在菇、Ｙ方向的物理尺寸为出、ｄｙ；ｕ。、 ％表示摄像头光心在图像上的投影坐标；这些参数
由离线的摄像头定标确定，其中（髯。ｚ。８。，）表示摄 像机相对于机器人的世界坐标，标定结果如表１所
为Ｏ（１０９２Ｎ）． 基于ＫＤ树的最邻近点搜索算法采用深度优先
的启发式搜索策略．用一种超矩形（ｈｙｐｅｒ—ｒｅｃｔａｎｇｌｅ） 数据结构来描述Ｋ维空间采样点集合所占据的空间 范围，表示为两个数组：一个数组存储每一维的最小 坐标值，另一个存储每一维的最大坐标值．判断一个
超矩形胁是否与以ｔａｒｇｅｔ为中心、以‘ｒ为半径的超球 体相交，首先需要找到府中最接近ｔａｒｇｅｔ点Ｐ，设胁
摘要：提出在基于单目视觉创建的环境地图中实现移动机器人全局定位．基于ＫＤ树的最近邻搜索实现特
征匹配．应用尺度不变特征变换（ＳＩＦＴ）方法提取特征，并用多维向量描述，保证了对图像光强变化、尺度缩放、三维
视角和噪声具有不变性．提出了一种基于ＲＡＮＳＡＣ的鲁棒定位方法．在实际室内环境Ｐｉｏｎｅｅｒ３机器人上进行的实验
维空问Ｒ×Ｄ中采样点集合，ｄ为目标点向量，则ｄ
的最邻近点ｄ’必须满足：
Ｖｄ”∈Ｅ，ｌ ｄｃ＝＞ｄ’｜≤ｌ ｄ副”｜
（１）
其中，｝ｄ甘ｄ’ｌ＝√∑：：，（ｄｉ一矗；）２，ｄｉ、ｄ ７；为向量ｄ和
ｄ’的第ｉ维元素．由于ＳＩＦＴ特征点描述器是１２８维 的，传统方法时间开销太大，因此提出了基于ＫＤ树 的最邻近点搜索算法来实现特征匹配，时间复杂度
征对进行三维重建才能获得，与基于立体视觉的地
图相比，精度存在差距．为了提高定位精度，需要充
分利用特征的图像坐标，即通过机器人ｔ时刻观测的
特征图像坐标与地图中的路标在某ｔ ７时刻的图像坐
标可以计算出机器人在这两个时刻的相对位姿关
系，实现相对定位，进一步可以确定机器人的全局位
姿．
３．１机器人相对位姿估计
示．
万方数据
１４２
机器人
２００７年３月
图３特征点的三维坐标估计
Ｆｉｇ．３ ３ Ｄ ｌｏｃａｔｉｏｎ ｏｆ ｋｅｙ－ｐｏｉｎｔ
Ｔａｂｌｅ １
表１摄像机内部参数与外部参数 Ｉｎｔｒｉｎｓｉｃ ａｎｄ ｅｘｔｒｉｎｓｉｃ ｐａｒａｍｅｔｅｒｓ ｏｆ ｔｈｅ ｃａｍｅｒａ
内部参数
外部参数
ａｚ
３６８．８２６２０ ｍｍ
万方数据
第２９卷第２期
厉茂海等： 基于单目视觉的移动机器人全局定位
１４１
征具有高度可辨别性，能更好地描述环境．ＳＩＦＴ算法 主要分为４步：（１）尺度空间极值检测；（２）特征点的
定位；（３）特征点角度计算；（４）特征点的描述．ＳＩＦＴ 特征点描述器用一个多维向量来表示，该向量包括 方向直方图中所有堆的值．本文采用４×４的直方图 数组，每个直方图有８个方向，因此每个特征点需要 用４×４×８＝１２８个元素的特征向量来描述。图１是 尺度不同（分别为１６１８ ｍｍ和７５６ ｍｍ）时对同一目 标进行特征点提取的结果，得到（ａ）２７８个特征点和
（ｂ）２６７个特征点，图中“＋”表示特征位置，本文所 有图像大小为３２０×２４０，从每帧图像提取的特征点
数一般在１０到３００之间． ２．２基于ＫＩ）树的ＳＩＦＴ特征匹配
ＳＩＦＴ特征点匹配的依据是特征点描述器（本文
为１２８维特征向量）之间的欧几里得距离最小．设一
个Ｋ维空间的定义域和值域分别为Ｒ和Ｄ，Ｅ为Ｋ
特征图像坐标，（ｕ∥，”∥，１）是地图中的匹配路标在 某￡’时刻的图像坐标．但是采用５点法求取Ｆ’矩阵 不稳定，对噪声很敏感本文采用ＲＡＮＳＡＣ方法来提
高Ｆ计算的稳定性： （１）创建匹配对列表．对当前图像帧提取的ＳＩＦＴ
特征寻找地图库中与之匹配的特征，并创建匹配列 表｛《¨一《，¨，…，Ｊ：Ｍ一髟们｝．
ｒ０一ｔ：０］ｒ ｃｏｓＯ ０ ｓｉｎＯｑ
Ｅ＝［明。Ｒ＝ｊ ｔ：０ 一ｔ，ｌｆ ０
１
０
ｌｏ￡。 ｏ ｊＬ—ｓｉｎ０ ０ ｃ。ｓｏｊ
（５） ［ｒ］。是ｔ时刻摄像头坐标系Ｃ到ｔ ｒ时刻摄像头坐标 系ｃ ７的平移矢量Ｔ＝［ｔ。０ ｔ：］Ｔ的反对称矩阵，Ｒ是
对应的旋转矩阵．显然，如果求解出基本矩阵Ｆ，那么 就可以分解出两个时刻的相对位姿．虽然Ｆ是一个 ３×３的矩阵，含有９个变量，但是只能在相差一个比 例常数的情况下确定Ｆ，而且Ｆ的行列式为０．在计 算机视觉中通常采用７个匹配对来求解Ｆ，每个匹配 对根据极线约束建立一个方程由于限定机器人在 平面上运动，则变换Ｆ为：
征变换（Ｓｃａｌｅ Ｉｎｖａｒｉａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ，ＳＩＦＴ）Ｅ２］提 取环境特征，在没有任何人为改变的室内自然环境 中实现机器人全局定位．基于ＫＤ树［３１的最近邻搜索 寻找ＳＩＦＴ特征匹配对，并三维重建［４ ３匹配对获得空 间三维路标．创建了具有可辨别性路标的环境模型． 提出了一种基于ＲＡＮＳＡＣ Ｅｓ］的定位方法．实验表明 本文方法能以较低计算开销、较高准确率实现全局 定位．
第２９卷第２期 ２００７年３月
文章编号：１００２－０４４６（２００７）０２－０１４０－０５
机器人ＲＯＢＯＴ
Ｖ０１．２９．Ｎｏ．２ Ｍａｒｃｈ．２００７
基于单目视觉的移动机器人全局定位
厉茂海１，洪炳镕１，罗荣华２，蔡则苏１
（１．哈尔滨工业大学计算机科学与技术学院，黑龙江哈尔滨１５０００１；２．华南理工大学计算机科学与工程学院，广东广州５１０６４０）
假设机器人在ｔ和ｔ ｒ时刻有南个特征匹配对，它
们的图像坐标分别为（ｔ¨，…，ｔ”）和（Ｄ“，…，
…
．
～，、ｍ ～ ，、
～
∥’），则根据极线约束满足：，∥仃∥＝０．Ｉ＝７（Ｍ，
移，１）１‘是归一化的图像坐标，Ｆ＝Ｃ１‘ＥＣ“是基本矩
阵，Ｃ是内部矩阵，Ｅ是本质矩阵，限定机器人在戈一ｚ
平面上运动，绕Ｙ轴的旋转角为０，则Ｅ表示为：
Ｋｅｙｗｏｒｄｓ：ｍｏｂｉｌｅ ｒｏｂｏｔ；ｇｌｏｂａｌ ｌｏｃａｌｉｚａｔｉｏｎ；ＫＤ—ｔｒｅｅ；ｆｅａｔｕｒｅ ｅｘｔｒａｃｔｉｏｎ；ＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）；
ｍｏｎｏｃｕｌａｒ ｖｉｓｉｏｎ
１引言（Ｉｎｔｒｏｄｕｃｔｉｏｎ）
移动机器人定位是机器人最基本、最重要的一 个功能，机器人的定位技术可分为位置跟踪和全局 定位两类．位置跟踪是指通过测量机器人相对于已 知初始位置的距离和方向来确定当前位置．而全局 定位要求机器人能够在对初始位置没有任何先验知 识的情况下，通过外部传感器感知信息确定位姿，因 此全局定位更具有挑战性．近来，视觉传感器因其能 感知丰富的环境信息而受到了普遍关注．许多已有 的视觉定位方法都采用人为设置路标的方法，并取 得了一定成功，如导游机器人ＭＩＮＥＲＶＡ¨１．对于不 允许人为改变的自然环境，特征提取方法是十分重 要的，决定了定位方法．本文基于便宜简单的单目 ＣＣＤ摄像机利用最近Ｌｏｗｅ等人提出的尺度不变特
２单目视觉实现（Ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｏｆ ｍｏｎｏ－ ｃｕｌａｒ ｖｉｓｉｏｎ）
２．１ ＳＩＦＴ特征提取 Ｌｏｗｅ等人提出的ＳＩＦＴ对图像的尺度缩放、旋
转、三维视角、光强的变化具有不变性，因此ＳＩＦＴ特
基金项目：国家８６３计划资助项目（２００２ＡＡ７３５０４１）；国家自然科学基金资助项目（６９９８５００２） 收稿日期；２００６—０４—２１
ｖａｒｉａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ（ＳＩＦＴ），ａｎｄ ｄｉｓｃｒｉｂｅｄ ｗｉｔｈ ｈｉｇｈｌｙ ｄｉｓｔｉｎｃｔｉｖｅ ｍｕｌｔｉ－ｄｉｍｅｎｓｉｏｎａｌ ｖｅｃｔｏｒ，ｍａｋｉｎｇ ｆｅａｔｕｒｅｓ ｂｅ ｉｎｖａｒ—
ｉａｎｔ ｔｏ ｃｈａｎｇｅｓ ｉｎ ｉｌｌｕｍｉｎａｔｉｏｎ，ｓｃａｌｅ，３Ｄ ｖｉｅｗｐｏｉｎｔ ａｎｄ ｎｏｉｓｅ．Ａ ｒｏｂｕｓｔ ｌｏｃａｌｉｚａｔｉｏｎ ｂａｓｅｄ ｏｎ ＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）ａｐｐｒｏａｃｈ ｉｓ ｐｒｅｓｅｎｔｅｄ．Ｅｘｐｅｒｉｍｅｎｔｓ ｏｎ ｒｏｂｏｔ Ｐｉｏｎｅｅｒ ３ ｗｉｔｈ ｍｏｎｏｃｕｌａｒ ＣＣＤ ｃａｍｅｒａ ｉｎ ｏｕｒ ｒｅａｌ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎ— ｍｅｎｔ ｓｈｏｗ ｔｈａｔ ｏｕｒ ｍｅｔｈｏｄ ｉｓ ｏｆ ｈｉｇｈ ｐｒｅｃｉｓｉｏｎ ａｎｄ ｓｔａｂｉｌｉｔｙ．
确率，本文对匹配对进行极线约束检测，可以忽略部
来自百度文库
分误差较大的匹配对．图２是基于本文提出算法对
不同方向和尺度的实验室一角进行特征匹配的结
果，共得到９３个匹配对，用“·”表示，匹配准确率在
８０％以Ｅ．
（ａ） Ｆｉｇ．１
（ｂ） 图１ ＳＩＦＴ特征提取 Ｔｙｐｉｃａｌ ｅｘｔｒａｃｔｅｄ ＳＩＦＴ ｆｅａｔｕｒｅｓ
（ｂ）
图２．基于ＫＤ树的ＳＩＦＴ特征匹配 Ｆｉｇ．２ Ｔｈｅ ｓＩｎ’ｆｅａｔｕｒｅ ｍａｔｃｈｉｎｇ ｂａｓｅｄ ０１３．ＫＤ—ｔｒｅｅ
２．３三维重建
如果机器人在ｔ，和ｔ：两时刻观察到路标点 Ｐ（ｘ。，Ｙ。，ｚ。），如图３所示．那么Ｐ的三维坐标与两
时刻的图像坐标Ｐ。（ｕ。，口，）与Ｐ：（Ｍ：，移：）的关系为¨１：
Ｘｅｒ
２．１０３９ ｍｍ
ｄ，
３６９．９０２３９ ｍｍ
０ｃｒ
ｕ０
１５９．６７０２９ ｍｍ
０。，
口０
１２１．５４１３６ ｍｌｒｌ
１００．１７４２ ｌｎｍ ９０。
３ 定位实现（Ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｆｏｒ ｌｏｃａｌｉｚａ— ｔｉｏｎ）
由于本文环境地图是利用单目ＣＣＤ摄像头创建
的，路标的三维坐标必须对两个不同时刻匹配的特