（信号与信息处理专业论文）基于光流法的机器人视觉导航.pdf

摘要 摘要 慕尼黑工业大学控制理论教席( l e h r s t u h lf u e rs t e u r u n g s u n d r e g e l u n g s t e c h n i k ) 下的城市自动搜索机器人( a u t o n o m o u sc i t ye x p l o r e r ) 项目 意在实现一个机器人平台，它能够在无序的城市环境中自动定位并通过和人类 的交流来选择前进的路径。为到达这一目的的一个前提条件是，机器人必须能 够精确的确定自己的位置，一般来说，底盘上轮子附近的角度传感器用来进行 导航；但如果机器人行走在不平坦的地面或沙地上，轮子将会发生打滑现象， 底盘导航就无法再提供精确的信息。 大量人体工程学的实时表明：视觉十分适合做为导航手段。视觉导航是通 过分析安装在机器人上的相机拍摄的一系列图片来估计机器人的运动：它是一 个本地的，低延时的闭环运动控制过程。它将帮助机器人相对精确的认知当前 的位置和转动信息。在这篇论文中将对几种计算光流的方法进行分析，比较， 进而选择效果最好的一种方法来估计相机的运动，然后建立模型来描述机器人 的运动，实现模型以达到机器人的最终定位。通过一系列合适的实验事件来实 时测试算法并评估结果。 最后，关于进一步工作的方向进行了简要的讨论。 关键词：光流计算，卡尔曼滤波，视觉导航 a b s 仃a c t a b s t r a c t t h ea u t o n o m o u sc i t ye x p l o r e r ( a c e ) p r o j e c to fi n s t i t u t eo fa u t o m a t i cc o n t r o l e n g i n e e r i n g ( l s r ) e n v i s i o n st o c r e a t e ar o b o tp l a t f o r mt h a tw i l la u t o n o m o u s l y n a v i g a t ei na l lu n s t r u c t u r e du r b a ne n v i r o n m e n ta n df i n di t sw a yt h r o u g hi n t e r a c t i o n w i t hh u m a n s t oa c h i e v et h i s ，t h ea c c u r a t el o c a l i z a t i o ni st h ep r e c o n d i t i o n a n g l e e n c o d e r so nt h ew h e e l so ft h ep l a t f o r ma r en o r m a l l yu s e df o rt h eo d o m e t r y b u ti fa c em o v e so nt h eg r o u n dw h i c hi sn o tf l a to rh a ss a n d s ，t h e ni tw i l ls l i p t h e e n c o d e r sc a nn o tp r o v i d ea c c u r a t ei n f o r m a t i o na n ym o r e s t r o n ge v i d e n c e sf r o mb i o l o g i c a ls y s t e m si n d i c a t et h a t ，v i s i o nc a n b ee x t r e m e l y v a l u a b l ef o rn a v i g a t i o n v i s u a lo d o m e t r yi st h ei n c r e m e n t a l ，o n l i n ee s t i m a t i o no f r o b o tm o t i o nb ya n a l y z i n gas e q u e n c eo fi m a g e sf r o ma no n - r o b o tc a m e r a i ti s d i s t i n c t l yl o c a l ，l o w - l a t e n c ya p p r o a c ht h a tf a c i l i t a t e sc l o s e d l o o pm o t i o nc o n t r o la n d h i 曲l ya c c u r a t ed e a dr e c k o n i n g i tw i l lh e l pa c e d e t e r m i n et h er e l a t i v e l yp r e c i s e p o s i t i o na n do r i e n t a t i o n i nt h i s t h e s i sav i s u a l o d o m e t r ys y s t e mf o r a c e ( a u t o n o m o u sc i t ye x p l o r e r ) i sd e s i g n e dt oe s t i m a t et h ec u r r e n tp o s i t i o no fa c e r o b o ti na nu n s t r u c t u r e du r b a ne n v i r o n m e n t t h ee x i s t i n ga l g o r i t h m so fo p t i c a lf l o w c o m p u t a t i o na r ea n a l y z e d ，c o m p a r e da n do n ea l g o r i t h mw i t ht h eb e s tp e r f o r m a n c ea r e s e l e c t e dt oe s t i m a t et h ec a m e r a ss e l f - m o t i o n s e v e r a lm o d e l sd e s c r i b i n gt h em o t i o n o fa c er o b o ta r es e tu pa n di m p l e m e n t e ds ot h a tt h el o c a l i z a t i o no fa c er o b o tc a nb e a c h i e v e d t h r o u g ha na p p r o p r i a t ed a t af u s i o na l g o r i t h mt h ew h o l eo d o m e t r ys y s t e m a r ei m p l e m e n t e di nr e a lt i m ea n de v a l u a t e do na c er o b o tw i t h i nap r o p e rs c e n a r i o k e yw o r d s ：o p t i c a lf l o wc o m p u t a t i o n ，k a l m a nf i l t e r , v i s u a lo d o m e t r y 学位论文版权使用授权书 本人完全了解同济大学关于收集、保存、使用学位论文的规定， 同意如下各项内容：按照学校要求提交学位论文的印刷本和电子版 本；学校有权保存学位论文的印刷本和电子版，并采用影印、缩印、 扫描、数字化或其它手段保存论文；学校有权提供目录检索以及提供 本学位论文全文或者部分的阅览服务；学校有权按有关规定向国家有 关部门或者机构送交论文的复印件和电子版；在不以赢利为目的的前 提下，学校可以适当复制论文的部分或全部内容用于学术活动。 学位论文作者签名：办1 d 、爵， 劢驴多年7 月s - e 经指导教师同意，本学位论文属于保密，在年解密后适用 本授权书。 指导教师签名： 学位论文作者签名：未l 寸、 年月 e 1 ) 口。铲年7 月谚日 同济大学学位论文原创性声明 本人郑重声明：所呈交的学位论文，是本人在导师指导下，进行 研究工作所取得的成果。除文中已经注明引用的内容外，本学位论文 的研究成果不包含任何他人创作的、已公开发表或者没有公开发表的 作品的内容。对本论文所涉及的研究工作做出贡献的其他个人和集 体，均已在文中以明确方式标明。本学位论文原创性声明的法律责任 由本人承担。 答名未j 心、 门1 i 、 签名：纱、j n n 、 2 。口缪年7 月) 箩日 第1 章引言 1 1 概述 第1 章引言 随着现代科学技术的发展，机器人在各行各业的作用越来越重要，按照人 体工程学的理论，机器人在整体结构上和人的相似性应当越来越高，和人一样， 视觉对于机器人来说也是不可或缺的一部分，相机对于机器人就想人的眼睛， 而计算机视觉处理也成为越来越重要的一门学科。 慕尼黑工业大学控制理论教席下的城市自动搜索机器人( 简称a c e ) 是要在 城市环境中智能的寻找自己的路径并到达目的地，视觉导航是用来帮助a c e 机 器人实现自身的精确定位，和人相似，通过视觉可以感知自己走过的路程，而 视觉导航就是为了实现这一目的。 1 2 视觉导航现状分析 经过三十余年的发展，视觉导航技术日趋成熟，早在八十年代，经典的光 流计算方法就已经提出，包括卢卡斯卡纳德算法和霍恩山克算法；到了二十 一世纪，用光流法来进行视觉导航并取得成功的例子越来越多。 杰森坎贝尔的模型中，相机安装在机器人的前端并斜下指向地面，光流 向量分为三部分：中间部分是盲区，不用于计算；水平线以上用于计算机器人 转动；水平线以下用于计算机器人的位移。和他的模型相似，在王教授的模型 中，相机只专注于地面，同时计算位移和转动。两个模型都是运动卢卡斯卡 纳德算法来计算光流值。内斯特运用哈里斯检测法选择特征点，实现了一个较 为精确但运行相对较慢的视觉导航算法。费尔南德斯在他的模型中运用了操作 系统的多任务运行能力，多帧运行的同步问题得到解决；为了达到更高的效率， 这里使用了s a d 算法。皮尔斯的模型中，视觉导航系统是工作在室内环境中， 他们假设所有的特征点在一个平面上。大多数视觉导航系统都是为一般的运动 物体进行导航的，比如汽车，机器人，这些实验平台都具有非线性系统的性质。 在n a s a 的实验室中，用于登月的机器人是一个线性系统，它可以向各个方向任 第1 章引言 意运动，对于太空中的复杂情况，视觉导航具有更大的优势。 1 3 提纲 本文的结构如下：在第二章将介绍一些必要的背景知识；在第三章是整个 系统的介绍，包括图像处理，动力学模型和数据融合算法；所有的实验过程和 实验结果将在第四章介绍；最后一章是结论和展望。 2 第2 章背景知i 【l 第2 章背景知识 奉章中，我们酋先引入非线性系统的概念，后文- - 所提到的动力学模型就是 肇j 非线性系统的性质而建t 的。然后，介销儿种计算光流的冉法和光学计算 的知识。最后，分析和研究f 卡尔曼滤波器的基本原理和性质。 21 非线性系统 本文的实验平台城l 】自动搜索机器人( a u t o n o m o u sc l t ye x p l o r o r ) 是一个底盘特有轮子的，能够自动行止和导航的智能机器人，如图所示 幽2i 城f 白动搜索机器人 和大多数汽车一样，在运动过程中，在机器人坐标系中相对于机器人相对 于自己j l 有嘶个使动吊，向前的线速度和以机器人重心为中心的角速度，但是 在世界单标系中机器人在垂直于前进方向的方向上足有位移的，所以这个系统 第2 章背景知识 可以被看作是一个非线性系统。最典型的非线性系统是独轮车，和独轮车相似， 机器人的最终状态是和它每时每刻的瞬时线速度和角速度有关的。 2 2 光流法 多年来，光流的计算一直是图像处理领域的一个核心研究问题，当今被公 认为最有效且被最广泛使用的有两类方法： 微分法 块相关法 2 2 1 块相关法 绝对值差别和方法( s u mo fa b s o l u t ed i f f e r e n c e ) 是块相关法中最重要 的一种，在这种方法中没个象素的位移可以被定义为 = p ，d yj ，我们也可将该 位移看作某一图像区域在不同时刻的变化。每一幅图片都可分为很多小的象素 块，对于在第一张图片的每个原始象素块，在第二张图片中都有相应的匹配象 素块于其对应，原始象素块和匹配象素块必须有最大的相似程度。卷积( c r o s s c o r r e l a t i o n ) ，平方值差异和( s u mo fs q u a r e dd i f f e r e n c e ) ，绝对值差异和都 可以作为相似程度的标准来评判。平方值差异和，绝对值差异和是两种最常用 的块相关方法，他们在在本质原理上是相同的，对于绝对值差异和，人们利用 原始象素块和匹配象素块中象素值的差的绝对值来进行比较，这些差值被加在 一起，拥有最小的绝对值差的候选块就是匹配块，而原始象素块和匹配象素块 之间的距离就是象素的位移 删d ( 戈；力) = w ( i ，) i ( 戈+ ( f ，j ) ) - 1 2 ( y c + d + ( i ，) ) l ( 2 1 ) j - 一nf ；一n 这个方程中y c = ( x ，y ) 是象素点在第一幅图片中的坐标，d = ( d ，d ，) 是原始 象素块的位移。在平方值差异和算法中人们只是用象素差的平方和代替了绝对 值和来做为比较标准。 s s d ( 童；0 ) = w ( i ，j ) i i ( i c + ( i ，) ) 一厶( 戈+ 0 + ( f ，埘】2 ( 2 2 ) 4 第2 章背景知识 2 2 2 微分法 微分法是另一种极为经典的光流计算方法，它是基于梯度限制的。这里人 们假设，在三维空间中的一个物体，当它移动或者静止不动时，它每个点的灰 度值是不变的，因此当它反应在二维的图片中的时候，在连续的两张图片中， 同一个象素的灰度值也是保持不变的，以上性质可以被描述为： ，( 戈，r ) = ，( 文+ 帝，f + 万，) ( 2 3 ) 其中帚= ( 6 x ，a y ) r ，物体在第一幅图片中的坐标是( x ，y ) ，光流计算的目的是要 在第二幅图片中找到经过了时间间隔所后该物体相应的位置 + 8 x ，y + a y ) 。然 后我们利用泰勒展开式来得到梯度限制方程 ， + 万毛j ，+ 万y ，f + 万f ) ：i ( x , y , t ) + 罢万z + 学万y + 祟研 ( 2 4 ) 6 c r y 仞 经过数学推导我们可以得到： 圪+ ，l = 一 ( 2 5 ) 其中v j ，是沿x 和y 方向的速度或光流的值，l ，和是图像灰度值在 ( x ，y ，f ) 的导数。但是，这里没有足够的方程来求解未知参数，我们可以称之为 “小孔问题 ( a p e r t u r ep r o b l e m ) 。所以人们必须加上一些其他的限制条件来 解决这个问题，霍恩山克设计加入一个光滑项来限制待估计的光流场，这个限 制可以被描述成： s = 儿呼【( 擎2 + ( 等) 2 + ( 豢) 2 + 芬) 2 d x d y ( 2 6 ) 另外，光流场限制： c = j l 孵( l 圪+ 0 + ) 2 d x d y ( 2 7 ) 为了计算速度场( 圪，吒) r ，应当将s + 们最小化，这里名是拉格朗日乘子。如 果图片的噪声很大，五应选取的足够小来获得更好的效果。霍恩山克算法的一 个比较明显的缺点是：该算法的速度很慢而且不是适用于实时的实验。 卢卡斯和卡纳德设计了另一种方法来加入限制条件，在一个所，l 的积分窗 口中，假设所有的象素点具有相同的光流值，因此总共有刀= 朋朋个速度相同 5 第2 章背景知识 的象素点，然后可得到一个超定方程组： l m i l ，r i 料 一厶 一： ： 一i m ( 2 8 ) 也可表示为： i 矽= 亡 ( 2 9 ) 为了求解这个超定方程，可使用最小二乘法，以伪逆矩阵的形式，+ ，可表达为 i + = ( i r ，) 一i r 。最后，光流向量可表示为： 矽= i + b = ( i r ，) 卅i r 亡 ( 2 1 0 ) 和霍恩山克法相比，卢卡斯卡纳德法更加准确且有更好的扛噪声性。在实际 中，如果物体的运动量很大，就应该尽可能选择较大的积分窗口；但与此同时， 较大的积分窗口又会导致错误匹配的可能性增大并会占用更多的计算资源。为 了解决这个问题，人们设计了用金字塔方法来实现卢卡斯卡纳德算法。 l e v e l0 f i n a lr e s u l td = d o + g o ( g 卜1 + d n - , ) 2 ( 9 8 - 2 + d n - 2 ) 图2 2 卢卡斯卡纳德的金字塔算法简图 6 第2 章背景知识 上图是金字塔卢卡斯卡纳德算法的基本流程图，这个算法的关键在于对光流 的迭代计算，它的好处是在匹配的时候可以达到更高的精度。对于一个金字塔 层数为的算法，原始图片，o 在最底层，也就是第“零 层，它的分辨率是a b 个象素；第一层的图像，1 是原始图片通过一个低通滤波器得到的，它的分辨率 是a 2 b 2 个象素；第3 ，4 ，一l 层的图片可定义为1 2 ，3 ，1 肛1 ，它们的象素分别为a 2 2 b 2 2 ，a 2 3 b 2 3 a 2 。1 b 2 肛1 。 光流的迭代计算是从金字塔的顶部开始的，在每一层，一个初始猜测会从上 层传递过来，用于本层的光流计算，这个初始猜测是根据上一层的计算结果产 生的。在金字塔的最顶层，初始猜测设为零：g 。1 = ( 0 ，0 ) ，然后根据基于梯度限 制的卢卡斯卡纳德方法计算光流d 。1 = ( 一，d 。n 。) ，。1 ( x ，y ) = i n - ! ( x + 谚，y + d 。1 ) ( 2 1 1 ) 传递到一2 层的初始猜测为g - 2 = 2 ( g 。1 + d 。1 ) ，在一2 的残余光流同样 根据梯度限制得到： i n - 2 ( x ，j ，) = i n - 2 ( x + g ，一2 + 谚，- 2y + g 了一2 + d y 一2 ) ( 2 1 2 ) 下面是在任意给定层的情况，比如在第三层，初始猜测可定义为 g 工= 2 ( g “1 + d “1 ) ，而在该层的残余光流通过下式计算： ，工( x ，y ) = i l ( x + + ，y + g ；+ d ；) ( 2 1 3 ) 在求得第零层的残余光流d 。后，最终的结果为： ，一l d = d o + g o = 2 工d 工 ( 2 1 4 ) 2 3 雅可比图像矩阵 位置估计的目的是要确定终端( 本文中相机为终端) 在世界坐标系中的位移 和旋转；雅可比图像矩阵描述的是相机在三维空间的速度和它自身图像平面上 的点速度的关系。这里户表示相机的速度向量，夕表示图像平面上的点速度向量： 户= 【弓zq 哆吐】2 ( 2 1 5 ) f = 陋矽】1 7 第2 章背景知识 其中l ，弓和t 分别是在空间世界坐标系中沿x ，j ，z 三个方向的线速度， q ，缈y ，c o ：为绕三个坐标轴的旋转角速度；西和多是图像沿x ，y 方向的点速度， 甜，1 ，则是相应的点坐标。，：和夕之间的关系可2 以由雅可比图像矩阵表示为： ：； = 兰o z o 兰 z uu v 允2 + v 2 z见允 ，名2 + 1 ，2z n ， z九九 其中名是相机的焦距z 是成像平面到地面的距离， 雅可比图像矩阵，可简化为： f = j 户 2 4 卡尔曼滤波器 t 弓 z q 伪y 哆 ( 2 1 6 ) 即相机的高度；如果用j 表示 ( 2 1 7 ) 1 9 6 0 年，卡尔曼发表了他著名的用递归方法解决离散数据线性滤波问题的论 文。从那以后，得益于数字计算技术的进步，卡尔曼滤波器已成为推广研究和 应用的主题，尤其是在自主或协助导航领域。卡尔曼滤波器用于估计离散时间 过程的状态变量x r ”。这个离散时间过程由以下离散随机差分方程描述： 黾= 么诈一l + b u k l + 叱一1 ( 2 1 8 ) 定义观测变量，得到量测方程： 气= h x k + 屹 ( 2 1 9 ) 随机信号和，。分别表示过程激励噪声l 和观测噪声。假设它们为相互独立，正 态分布的白色噪声： p ( w ) ( 0 ，9 ( 2 2 0 ) p ( ，) n ( o ，r ) 、。 实际系统中，过程激励噪声协方差矩阵9 和观测噪声协方差矩阵r 可能会随每 次迭代计算而变化。但在这儿我们假设它们是常数。当控制函数u 或过程激励 噪声峨一。为零时，差分方程1 1 中的刀刀阶增益矩阵彳将过去k - 1 时刻状态和现 8 第2 章背景知识 在的k 时刻状态联系起来。实际中彳可能随时间变化，但在这儿假设为常数。刀， 阶矩阵曰代表可选的控制输入u r 。的增益。量测方程1 2 中m 聆阶矩阵日表示 状态变量以对测量变量z 。的增益。实际中h 可能随时间变化，但在这儿假设为 常数。 以上是卡尔曼滤波器的总体性概述，在光学领域及状态预测，离散卡尔曼滤 波器应用的十分广泛，下面我们讨论离散卡尔曼滤波器的具体细节及其作用。 卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的 状态，然后以( 含噪声的) 测量变量的方式获得反馈。因此卡尔曼滤波器可分 为两个部分：时间更新方程和测量更新方程。时间更新方程负责及时向前推算 当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。 测量更新方程负责反馈一一也就是说，它将先验估计和新的测量变量结合以构 造改进的后验估计。时间更新方程也可视为预估方程，测量更新方程可视为校 正方程。最后的估计算法成为一种具有数值解的预估一校正算法。在时间更新 部分，状态变量根据时间的变化和上一次的状态量向前推进： 状态估计x i 和协方差估计巧从k 一1 时刻向前推算到k 时刻；在状态更新部分， 首先计算卡尔曼增益k 。，然后测量输出以获得z 。，接着产生状态的后验估计， 最后估计状态的后验协方差： k t = p h i0 h h l + 脚1 龟= + q ( 气一硪i ) ( 2 2 2 ) e = ( ，一墨- ) e ； 计算完时间更新方程和测量更新方程，整个过程再次重复。上一次计算得到 的后验估计被作为下一次计算的先验估计。这种递归推算是卡尔曼滤波器最吸 引人的特性之一一一它比其它滤波器更容易实现：例如维纳滤波器，每次估计 必须直接计算全部数据，而卡尔曼滤波器每次只根据以前的测量变量递归计算 当前的状态估计。下图可以更加直观和清晰的描述整个过程： 9 俎q 脚哪 + m惋以 = = 巧 第2 章背景知识 涮璧更新( 校正) 时间更辆( 预测) ( 1 ) 计算卡尔曼增益 ( 1 ) 向前推算状怒变麓 k 。= p i i h 。 h p f h + r i - i f = a 叠扣，中b t k l ( 2 ) 由观溅变耆酝。更胬；健诗 ( 2 ) 向靛搓算误差协方差 氛* + k z ；i h 叠k 一 致一= a p k 1 a 5 伯 3 ) 更新误蓑协方蓑 气= f i - k k h ) p k 一 j 一 敏一，和p h 为初始估计 图2 3 卡尔曼滤波器的基本过程 1 0 第3 章系统介绍 第3 章系统介绍 我们的实验平台一一城市自动搜索机器人( 简称a c e ) 的主要功能是探索室 外的城市环境，并且要频繁的与人交流，所有用于视觉导航的相机不能直接对 着前方。为了避免在相机视野里频繁的出现移动的物体以导致对导航的影响， 相机安装在机器人的前方，光轴垂直于地面，如图所示 y r w 图3 1 机器人一相机模型的侧视图( 左图) 和俯视图( 右图) 3 1 硬件及软件 首先我们简单介绍一下我们实验的硬件配置和相应的软件系统。实验中我 们使用的是d r a g o n f l ye x p r e s s 相机，它是传统的d r a g o n f l y 相机的一个扩展， 在保留了大多数传统d r a g o n f l y 相机的优点的同时，它还使用了 i e e e 一1 3 9 4 b ( f i r e w i r e $ 8 0 0 ) 接口来达到更高的数据传输速率。它的最高工作频 率为2 0 0 帧秒，分辨率为6 4 0 x 4 8 0 象素。1 3 9 4 b - p c i 适配器用于将从相机获得 的信息传递到安装在机器人上的视频处理计算机。处理视频信息的计算机配备 了a m d 9 5 0 0 四核芯片，主频是2 2 g h z ，内存4 g b 。相应的软件程序都是用c c + + 实现的，其中一些必要的函数可在o p e n c v 下。 第3 辛系统介纠 窿 i 警| 32a c e 实物圈( 左幽) 午相机实物图( 也幽 32 算法 首先大致的算法流程蓟】图33 所示，在这一章我们分别介绍算法的细竹 3 21 光流计算 在我们的模型中图像坐标系是平行于地面，x 轴和y 轴的正方向和相机坐标 系的正与向相同。相机坐标系的z 轴正方向就是光轴的方向并和地面垂直。如图 34 所乐。 和其他的计算光流的山法相比，绝对值差别和算法( 以后简s a d ) 工作效率 更高并且占用更少的系统资源。我们从相机获得的图片的大小足6 4 0 x 4 8 0 个象 素，中【j j 的4 0 0 x 4 0 0 个象素被选为感* 趣区域( 如图中所示的i n t e f e s ta r e a ) ， 每2 0 x 2 0 个象素定义为个搜索窗口，凼此在感兴趣区域共有4 0 0 个搜索窗u ， 把每个窗口罩中央的8 x 8 个蒙素看成一个块，这个块可以看作是在第”1 帧上 的原始块，在第月帧的相应区域附近的匹配候选块分别计算和原始块的扶度值的 睁悖蘑 理e鞭。翻y莲 第3 章系统介绍 图3 3 算法大致流程 1 3 第3 章系统介绍 成像 t ；x f l ； y c j 多 i 以u ，v ) ，t 一 。欧奴一 v ： i ! 标系 ： l 例 、 图3 4 坐标系描述 u 差的绝对值的和，即进行s a d 算法的运算；下一步，绝对值的和最小的那个候 选块可就是我们要寻找的匹配块；原始块和匹配块之间的距离就是这个搜索窗 口的光流值。 图3 5 优化后的s a d 算法 1 4 第3 章系统介绍 由于共有4 0 0 个窗口，在s a d 匹配后我们得到了4 0 0 组光流值，但实际情 况中，由于噪声的影响，会有一些匹配错误，相应的光流值也是错误的，所以 我们采取下面的方法将这个s a d 算法继续优化：如图3 5 所示，剔除在感兴趣 区域的边界上的搜索窗口的结果，因为在边界上有很大的可能性会匹配错误； 剩余还有1 8 x 1 8 个搜索窗口，我们将他们分为3 6 组，每组包含3 x 3 个窗口；在 每组内我们设置了一个标准，不符合这个标准的窗口也将被剔除掉，而其余的 窗口的光流值的平均值作为这一组3 x 3 大窗口的有效光流值。实际上，我们可 以将每一组看做一个象素，我们只是以更高的精确度求得了3 6 个点的光流值。 除了s a d 算法我们还尝试了卢卡斯卡纳德的金字塔算法来求得光流值， 在o p e n c v 库下可以使用这个函数。由于这个算法的计算复杂程度较高，和s a d 算法相比它运行的比较慢。这里我们将最大金字塔层数设为3 ，在每一层积分窗 口的大小设为7 x 7 ；卢卡斯卡纳德算法可以在每层上处理7 个象素的运动，通 过金字塔实现后，算法能处理的最终象素运动为以。= ( 2 3 一1 ) 7 = 1 0 5 个象素。 如果机器人以最低速度0 2 m s 运动同时相机工作在3 0 帧秒，那么连续的两张 图片之间的象素移动为1 3 8 2 1 象素帧；如果机器人以最高速度l m s 运动同时 相机工作在3 0 帧秒，那么连续的两张图片之间的象素移动增至6 9 1 0 5 象素 帧；由于硬件条件的限制在一些情况下会出现跳帧情况，两张连续的图片之间 会有很长的时间间隔。因此在成像平面上的象素移动就会更大，这个卢卡斯卡 纳德金字塔算法可以处理的最大象素移动是1 0 5 ，它已经足够稳定处理大多数的 跳帧情况。我们选择了不同的金字塔层数和积分窗口的大小，然后比较其运算 速度和结果的精度( 见表3 1 ) 表3 1 选择不同层数不同窗口大小时计算速度和精度的比较 层数窗口大小 平均计算时间( a s ) 误差( ) 47 x 7 1 6 1 3 51 0 4 3 3 7 x 71 3 5 1 41 0 4 5 2 7 x 71 0 3 0 24 7 1 5 45 x 51 1 4 0 61 2 0 5 35 x 59 7 4 9 1 4 3 25 x 5 7 7 6 21 7 9 0 从表3 1 我们可以看出如果选择5 x 5 的积分窗口大小，光流的值将很有可 1 5 第3 章系统介绍 能超过算法能够处理的最大象素移动，因此选择7 x 7 的窗口大小比较理想；如 果选择金字塔层数为4 ，算法将很耗费时间，精度的提高却很有限，如果选择层 数为2 ，又将大大降低最大象素移动。最后，选择7 x 7 的积分窗口和3 层金字塔 的卢卡斯卡纳德算法是最好的选择。 3 2 2 运动估计 通过一个合适的计算光流的方法得到光流值后，用光流值除以每两帧的时 间间隔我们可以得到在成像平面上特征点的点速度。然后我们应用卡尔曼滤波 器和雅可比图像矩阵来估计相机的速度。在我们的实验中，我们假设成像平面 和地面是时时刻刻保持完全平行，相机和地面的距离是保持不变的，所以在雅 可比图像矩阵中的疋，缈，国，都可以近似的看为恒等于零，我们的模型也从6 个 自由度( 6 d o f ) 简化为3 个自由度( 3 d o f ) 。因此在我们的模型中雅可比图像矩阵 可简化为： 1 ，l ： u 3 6 屹6 兄 一0 一h z 名 0 一地 z 名 一0 一b 6 z a 0 一甜3 6 z ( 3 1 ) 正如上一节所提到的，我们选择了3 6 个特征点，因此测量向量包含7 2 个 元素，而由于模型简化为3 个自由度，状态向量只包含3 个元素。在我们的实 验中卡尔曼滤波器的基本过程可表示如下： 2 黾一1 + r q9 、 u 厶， z k = 以+ 唯 每一步的运动估计中，后验状态估计就是我们要求的这个方程的解，因此 通过卡尔曼滤波器我们可以将超定系统的冗余滤掉并得到相机沿x ，y 方向的线 速度和绕z 轴旋转的角速度。 在o p e n c v 库下有已经实现的离散卡尔曼滤波器函数，o p e n c v 提供了一个结 1 6 第3 章系统介绍 构体c v k a l m a n 来存储滤波器的状态，同时也有提供相应的函数来创建和更新滤 波器的状态。在我们的实验中我们试图不使用o p e n c v 提供的函数，在提高运算 速度的同时到达相同的计算精度。表3 2 显示了两种不同的卡尔曼滤波器的运 算时间： 表3 2 两种不同的卡尔曼滤波器的运算时间比较 条件 最大时间( m s )最小时间( m s )平均时间( m s ) 使用o p e n c y 函数 5 9 1 7 92 7 4 33 0 1 5 不使用o p e n c v 函数 4 1 1 4 01 5 2 61 5 9 8 由上表可以看出，使用我们自己编写的卡尔曼滤波器程序，运算时间几乎 减小了一倍，但实验结果表明，精度几乎相同，所以我们已经在没有损失运算 精度的情况下提高了运算速度。 3 2 3 最小二乘法 在使用卡尔曼滤波器求得了相机运动后，基于非线性系统的性质我们建立 了一个超定系统来描述相机速度和机器人速度的关系( 见下一章) 。在实时实验 种，我们也可以选择伪逆矩阵来代替卡尔曼滤波器，以达到提高实时运算速度 的效果。实际上，伪逆矩阵是基于最小二乘的原理的，它的目的是求得模型的 一个最优解；这个最优解应当满足，残余平发差的和应该达到最小值。比如在 我们的模型中，雅可比图像矩阵可表示为尹= ，户，最小二乘法能够求得，：的最 优解，满足向量的模i 广一，户i 最小，通过伪逆矩阵的形式，+ ，j + = ( r ) 1 j7 我 们可以求得户： 户= j + 夕 ( 3 3 ) 基于最小二乘法和雅可比图像矩阵我们可以得到： 哆( 坼一刃+ 喀( - 一v ) 皱：= j 三l 可竽虿一 笪丝 ( ) 2 ( u ) 2 ( 3 4 ) 荟彳+ 善谚一一 1 7 第3 章系统介绍 3 3 动力学模型 3 2 1 单相机模型 乃，= 乙= 3 63 6 z t z 喀+ v f 缈】 3 6 五 3 63 6 z 嘭一“j 国】 3 6 力 ( 3 5 ) 当我们建立动力学模型的时候，我们需要主要坐标转换的问题。对于机器 人坐标系，x 坐标系的正方向是机器人前进的方向，在相机坐标系中，x ，y 的正 方向分别是机器人坐标系的y ，x 的正方向的反方向。图3 6 给出了在三维空间中 相机坐标系和机器人坐标系的关系： 系： 图3 6 在世界坐标系中两帧之间的相机位移 机 下图是在世界坐标系中机器人的位移和在机器人坐标系中机器人位移的关 1 8 第3 章系统介绍 a x c 图3 7 在世界坐标系中两帧之间的相机位移 每次我们应用卡尔曼滤波器或最小二乘法来估计相机的速度，我们可以得 到相机在每一时刻的瞬时速度。在世界坐标系中两帧之间的相机位移和相机的 当前的角度和x ，y 方向的线速度有关( 见图3 7 ) 。实际上这个问题可以转化为 相机坐标系和世界坐标系的转化问题： 这里血。和缈。是在世界坐标系中相机的位移，址是两帧之间的时间间隔，在力帧 之后相机的位移是： 艺= 缸 1 ( 3 7 ) 儿= 蚬 l 下面我们可以通过机器人和相机之间的几何关系来估计机器人的转动和位 移。在整个过程中机器人和相机之间的相对位置是不变的，因此i 口，i 爿口。l ；又 由于相机坐标系和机器人坐标系的正方向是相反的，因此口，= - - t ；t 。；机器人的 任何使动运动都将导致相机相对自身起始点的位移和转动。实际上机器人同时 存在位移和转动，为了简化我们的数学模型，机器人的运动可以分成两部分。 首先，它没有位移而只有一个转动口，但这时相机有一个位移( rx 。，y 。) 。第二 步，机器人有一个位移( x ，”) ，此时相机将产生一个位移( x 。，y 。) = ( - y ，一x ，) ， 1 9 劬 尉 缸 、，、-、 洫- 口 s s 乙乙 一 + 瞄 啷 工 y 乃乃 | i = 麟姚 第3 章系统介绍 因此我们得到： cx c y r = x c yc x r = y c 图3 8 单相机基于位移的模型中的坐标几何关系 ( 3 8 ) 从图3 8 我们可以推出。x 。= r s i n ( p + a 。) - y o 并且。y 。= x o r c o s ( p + a 。) ， 最后机器人的位移和转动可表示为： y rl - r s i n ( , a + a 3 一y o x c = x o r c o s ( + 晖) 一咒 ( 3 9 ) q r 2 一q c 在这个模型中我们首先计算了相机的位移和转动，然后根据相机和机器人 的几何关系来求出机器人的位移和转动，因此这个模型可以被称作基于位移的 模型。 在上一章我们曾经介绍过，a c e 机器人可以看作是一个非线性系统。显然在 2 0 第3 章系统介绍 每一时刻，相机的瞬时速度和机器人的瞬时速度有一个关系；因此，当我们得 到相机的瞬时速度时，应用非线性系统的性质我们可以直接求得机器人的瞬时 速度，如图3 9 所示，在每一时刻，机器人只有一个向前的线速度z ，和角速度 国，z 。 f ： ： t 。工r x r r z x r 。 f 叭 t 工c x t 1 c y t r ： 飞r x 0 7 y r ( 。) r z 。，一? 1 图3 9 单相机基于速度的模型中的坐标几何关系 将整个系统看作一个非线性系统，系统的输入只有两个，线速度z 。和角速 度彩，。在整个过程中，机器人的重心和成像平面的中心的相对位置不变；如果 机器人向前的速度是z 。，相机同时也有一个向前的速度l 。：机器人如果有一个 角度国这将导致相机产生一个垂直于半径r 的线速度r c o ，：。这可以表示为： t o 工= 一o ) r z r c o s = 一啡：x 0 ( 3 1 0 ) 乏，= 一t r ，一c o r ：r s i n p = 一i ，一q ：y o 最终相机速度可表示为： 瓦， q z i z ，= c ，y o 一乏， 通过弓，和缈，：我们可以计算机器人的位移和转动， 2 1 ( 3 1 1 ) 我们假设机器人的运动轨迹 第3 章系统介绍 如图3 1 0 所示： 一_ _ _ 叫 ! - y 2 3w y l 2 图3 1 0 机器人在世界坐标系中的运动轨迹 显然机器人在世界坐标系中沿x ，y 方向的位移完全取决于机器人在机器人 坐标系中沿x 轴的位移和转动的角度。在时刻拧，线速度，角速度和机器人的转 动角度可表示为z ，( 咒) ，国，：( 刀) 和已，时刻n - 1 和r 之间的位移是x 川。和 w y 州下面是具体的计算过程： 3 2 2 l x x 一 一 霄 毗 一k i 第3 章系统介绍 ”而l2 珊) t 1 2c o s q ( 3 1 2 ) ”毛一l ，。= 乃，( 刀一1 ) 乙一l 。c o s 秒1 截止到时刻门机器人在世界坐标系下的位移为以，e ，实际上，这是一个积分 见= 喜州川。= e 嘣触 w 以= 喜。= 肛c 。s 渺 ( 3 1 3 ) ”匕= 喜、= 肛s i n 渺 结果并不理想，那是因为我们忽略了一个潜在条件缈，：= - - ( 0 ，。，这也是超定系统 中的一个冗余限制条件；这里户代表相机的速度向量，夕代表成像平面的点速度 户= 【l 哆r ( 3 1 4 ) 夕= 【玩峨疗z z i 3 c 也s r( 3 1 5 ) 户= 户 第3 章系统介绍 为： 这个变换矩阵我们可称之为日。础咖，且是雅可比图像矩阵，则最终的方程 3 2 2 双相机模型 f = j ，：= j i - i 砌d ，d 乡 ( 3 1 7 ) 图3 1 1 双相机模型的侧视图( 左图) 和俯视图( 右图) 使用单相机模型时，我们发现转动角度的结果不够准确，这是因为在成像 平面中，点坐标之间的差异远远小于机器人重心到成像平面中心的距离，因此 雅可比图像矩阵对转动并不敏感。我们考虑再添加一个相机来完善我们的模型， 这样角度的估计就更加准确。 我们在a c e 机器人的左下角再增加一个d r a g o n f l ye x p r e s s 相机，如图 3 1 1 所示。首先是基于位移的双相机模型，我们可以分别计算出两个相机的位 移x c 。，y d ，x c ：，y c ：，然后分别建立起相机位移和机器人位移的关系： = 而一墨c o s ( p 1 一q ) 一y c l y r = 局s i n ( 层一口r ) 一y l t 1 ( 3 1 8 ) x r = re o s ( p 2 一嘭) 一x 2 一儿2 ”= 儿一是s i n ( f 1 2 一晖) 一2 然后我们可直接根据以上四个方程数学推导出机器人的位移和转动。这里 为了简化结果，我们假设：k r = x i + x 2 ，k j ，= y l + y 2 ，a = y l + y 2 + t l x 。2 ， 2 4 第3 章系统介绍 b = x l + x 2 + 虬2 一y c l ，接着转动角口，可表示为： c o s q = 鬻 血q = 等等 ( 3 1 9 ) 图3 1 2 双相机模型中相机位移和机器人位移的几何关系 在求得转动后，位移可以通过公式得到。在这个模型中我们并没有用到条 件口，= - t ：t 。，因此结果将比单相机模型更准确。 在双相机模型中，我们同样可以应用基于速度的模型理念。如图3 1 3 所示： 2 5 k h ，d 一 赢 ；一 f0i ，。、，。 h c x 、|，r。以 第3 章系统介绍 t r x t c y 2 p 口 工厶 t e x l 图3 1 3 双相机模型中相机速度和机器人速度的关系 在基于速度的模型中，我们应用非线性系统的性质，通过相机的速度来求 机器人的速度。增加一个相机之后，我们相当于增加了额外的三个限制条件到 这个超定系统中，增加了系统的冗余度，通过最小二乘法我们可以得到一个更 为优化的解。方程组有两个未知数，六个方程： 瓦善l = 一缉：而 乃订= 一霉，一q ：乃 嚷z l q ： ( 3 2 0 ) 2 _ 2 = q ：恐 j ，2 = 一霉，+ q ：y 2 岔k ：22 一国，： 其中疋小瓦y ，疋伽疋y ：是两个相机在各自相机坐标系中沿x ，y 方向的线速 度；( - 0 c 小彩啪分别是两个相机的角速度。我们将方程组写成矩阵运算的形式： 第3 章系统介绍 0 一五 一1 一m 01 0 而 - 1 y 2 01 ( 3 2 1 ) 这还可以表达为：之= 三毫，然后我们也是应用伪逆矩阵来计算r ，接着继续 可得到：毫= 矿之，除去了冗