（信息与通信工程专业论文）基于视觉定位的地图构建方法研究.pdf

浙江大学硕士学位论文 摘要 机器人视觉导航是机器人领域研究的热点，其中视觉定位、感知及地图构建 算法是视觉导航的核心与关键。基于视觉的定位与地形感知方法具有精度高、能 耗小等优点，尤其视觉定位算法能够进行准确的全局定位，不会像惯性导航装置 受到累积误差影响或像轮式里程计受到车轮打滑等不利因素的影响。因此本文设 计提出了基于视觉定位的致密地图构建方法以及整体视觉导航系统方案。其中视 觉定位算法使用基于双目立体视觉的运动估计算法，使用s i f t 算法进行帧间特 征跟踪，使用r a n s a c 算法去除误匹配并使用最小二乘法进行运动估计，从而 得到机器人精确的全局坐标和路径轨迹。本文还提出基于联合置信度的改进种子 像素扩张算法，改善了图像边缘区域的视差扩张计算结果，提升了原有视差扩张 方法的整体性能以及局部地形三维重建的效果；在以上算法的基础上，本文还提 出了一种基于自标定的宽基线立体匹配方法用于远距离地形三维重建。在以上视 觉定位和地形三维重建的基础上，本文设计了一种基于栅格d e m 的地图构建方 法，将三维重建得到的地形三维点云图投影到二维栅格地图并计算每个栅格对应 地形的高度，基于综合高度阈值判断以及备选坡度估计进行通行性判定并生成通 行性地图。最后根据获得的通行性地图进行动态路径规划，从而引导机器人遵循 规划路径规避所有障碍物地形并最终到达目的地。通过一系列不同光照、纹理、 地形的室内外测试场景实验，验证了本文所提出的视觉定位算法、立体视觉三维 重建方法、地图构建方法以及整体视觉导航系统的有效性和鲁棒性。 关键词：视觉导航、视觉里程计、立体匹配、地图构建 2 浙江大学硕士学位论文 a b s t r a c t v i s u a ln a v i g a t i o ni sah o t s p o to fr o b o t i c s ，a m o n gw h i c hv i s u a ll o c a l i z a t i o n , p e r c e p t i o na n dm a p p i n ga r ea l lt h ec o r ei s s u e s v i s i o nb a s e dl o c a l i z a t i o na n dt e r r a i n p e r c e p t i o nm e t h o dp o s s e s s e st h ea d v a n t a g e so fh i 曲a c c u r a c ya n dl o wp o w e r , e s p e c i a l l yt h ev i s u a lo d o m e t r ya l g o r i t h mo w n sl i u l ea c c u m u l a t i v ee r r o rc o m p a r e d w i t hi m uo rw h e e lo d o m e t r yl o c a l i z a t i o n h e n c e ，t h i sp a p e rp r o p o s e dav i s u a l l o c a l i z a t i o nb a s e dd e n s em a p p i n gm e t h o da n dav i s u a ln a v i g a t i o ns o l u t i o n t h ev i s u a l l o c a l i z a t i o na d o p t sb i n o c u l a rm o t i o ne s t i m a t i b nt oo b t a i ng l o b a ll o c a l i z a t i o na n d t r a j e c t o r y , w h i c hu s e ss i f tt ot r a c kf e a t u r e sb e t w e e nf r a m e s ，u s e sr a n s a ct o r e m o v em i s m a t c h i n ga n dl e a s ts q u a r em e t h o dt oo b t a i nm o t i o ne s t i m a t i o n t h i sp a p e r a l s op r o p o s e dj o i n tr e l i a b i l i t yb a s e d d i s p a r i t ye x p a n s i o na l g o r i t h mt oi m p r o v e e x i s t i n ga l g o r i t h m sp e r f o r m a n c e t h i sp a p e ra l s od e s i g n sas e l f - c a l i b r a t i o nb a s e d w i d e b a s e l i n es t e r e om a t c h i n gm e t h o df o rl o n gr a n g et e r r a i nr e c o n s t r u c t i o n m o r e o v e r , a 酊db a s e dm a p p i n gm e t h o di sd e v e l o p e dt ob u i l dd e mm a pa n dp e r f o r mt h e e l e v a t i o nt h r e s h o l d i n ga n do p t i o n a ls l o p ee s t i m a t i o nf o rt r a v e r s a b i l i t ya n a l y s i sa n d t r a v e r s a b i l i t ym a pb u i l d i n g f i n a l l y , d y n a m i cp a t hp l a n n i n gm e t h o di su s e dt og u i d e r o b o ta v o i da l lo b s t a c l e sa n da r r i v et h ed e s t i n a t i o n w i t hs e r i e so fe x p e r i m e n t s ，t h e v i s u a ln a v i g a t i o ns o l u t i o np r o p o s e di nt h i sp a p e ri sp r o v e dt ob ef e a s i b l ea n dr o b u s t k e y w o r d s ：v i s u a ln a v i g a t i o n ，v i s u a lo d o m e t r y , s t e r e om a t c h i n g ，m a p p i n g 浙江大学研究生学位论文独创性声明 本人声明所呈交的学位论文是本人在导师指导下进行的研究工作及取得的研究成果。 除了文中特别加以标注和致谢的地方外，论文中不包含其他人已经发表或撰写过的研究成 果，也不包含为获得逝姿盘堂或其他教育机构的学位或证书而使用过的材料。与我一 同工作的同志对本研究所做的任何贡献均已在论文中作了明确的说明并表示谢意。 一躲五峰珏嗍妙年圳日 学位论文版权使用授权书 本学位论文作者完全了解澎望太堂有权保留并向国家有关部门或机构送交本 论文的复印件和磁盘，允许论文被查阅和借阅。本人授权逝江太堂可以将学位论文的 全部或部分内容编入有关数据库进行检索和传播，可以采用影印、缩印或扫描等复制手段 保存、汇编学位论文。 ( 保密的学位论文在解密后适用本授权书) 一躲盈姊；逸 新躲愀 签字日期乃舻弓月乡日 签字嘲刀f o 年岁月1 7 日 浙江大学硕士学位论文 致谢 时光荏苒，两年半的硕士研究生学习生涯转瞬即逝。回顾近两年多的学习生 活，我有幸师从刘济林教授。刘老师治学严谨、学识渊博，对计算机视觉、机器 人学、自动化等学科前沿有着敏锐的眼光和独到的见解；刘老师的严格要求和精 心指导引领我在科研学习中不断成长；而刘老师踏实严谨的工作作风和一丝不苟 的科学态度必将使我受益终生。在此对刘老师致以最诚挚的感谢。 读研期间，周文晖老师具体指导我的科研工作。周老师为人热心诚恳，具有 很强的专业洞察力和科研能力，并悉心指导我适应研究生的学习、工作和生活。 即使在奔赴新的工作岗位之后，周老师仍十分关心我的科研进展和个人发展。我 取得的每一点成绩也都离不开周老师的关怀与帮助。此外，杜歆老师、项志宇老 师、谢立老师、李培泓老师以及龚小瑾老师也都给予了我莫大的关心和指导。在 此也对给予过我关心和帮助的各位老师致以衷心的感谢。 由衷感谢实验室课题组的沈晔湖、沈敏一、徐进、杨力，各位博士涉猎广泛， 对计算机视觉、软件工程等相关领域有着很深入的研究，对我研究过程中涉及的 各个相关课题都提供了极大帮助。 还要感谢实验室以下各位同学：潘华东、李智、雷杰、张雷、杨鹏、姚拓中、 王选贺、陈曦、丁菁汀、杨镔、丁鑫、陈建娟、王延长、林颖、李永佳、卢伟、 路丹辉等。感谢我的室友王万丰、杨名、徐鲁威、孙伟。大家近三年来的朝夕相 处，给予我很多美好的回忆。 感谢父母的养育之恩，以及很多亲人朋友在我读研期间给与的关心与帮助， 难以尽述，在此一并致谢。 王炜强 2 0 1 0 年1 月于求是园 浙江大学硕士学位论文 第1 章绪论 1 1 课题研究背景 机器人自主视觉导航是目前世界范围内人工智能、机器人学、自动控制等学 科领域内的研究热点。传统机器人自主导航依赖轮式里程计、惯性导航装置 ( i m u ) 、g p s 卫星定位系统等进行定位。而轮式里程计在车轮打滑情况下会产 生较大误差，惯性导航装置( i m u ) 在长距离导航中受误差累积影响会造成定位 精度下降，g p s 定位技术在外星球探测或室内封闭环境应用中受到诸多限制。因 此，基于双目立体视觉的定位算法成为解决轮式里程计和惯性导航装置定位误差 的可行方法。另一方面，机器人自主导航需要对周围环境进行实时动态的感知和 重建，并构建地图用于导航和避障。传统的地形感知多使用激光雷达、声纳、超 声、红外等传感器及相关方法，激光雷达功耗、体积较大，不适用于小型移动机 器人，而超声、红外传感器作用距离有限且易受干扰，而采用被动光学传感器的 视觉方法，体积功耗小，信息量丰富，因此基于视觉方法进行地形感知与地图构 建具有广阔的应用前景。综上所述，基于视觉的定位、地形感知及地图构建方法 具有功耗低、体积小、应用范围广泛等特有优势，本文将专注于研究基于视觉技 术的陆地移动机器人定位、地形感知、地图构建方法，实现鲁棒、高效、可扩展 的陆地移动机器人自主导航系统。 1 2 视觉导航技术现状 1 2 1 自主导航技术 目前，陆地移动机器人自主导航技术仍然是机器人领域研究的热点，并在最 近几年取得了一系列举世瞩目的成就。 美国斯坦福大学研制的陆地无人自主车s t a n l e y 1 】( 如图1 1 ) 使用融合了激 光雷达、被动视觉、g p s 等技术的导航方案，在2 0 0 5 年美国国防高级研究计划 局( d a r p a ) 举办的无人车自主越野“大挑战”( g r e a tc h a l l e n g e ) 仅用时6 小 6 齑太学硬士掌位论文 囤i - 2 “机遇”号火星探测墁游车 美国太空署( n a s a ) 联合喷气动力实验室( j p l ) 所研制的火星探利漫游 车( m e r ) 。勇气” s p i r i t ) 和“机遇”( o p p o r t u n i t y ) 号【2 ( 如宙卜2 ) ， 于2 0 0 4 年着陆火星这两部火星车都使用了主要基于视觉的导航技术，每部车 体上都安装了三组立体视觉系统，包括车体前后备一组用于检剥并规避障碍物的 立体祝觉系统，以及一部安装在丰体桅杆顶部的用于导航的立体视觉东统车载 7 浙江大学硕士学位论文 计算平台为主频为2 0 m h z 的r a d 6 0 0 0 处理器，能够基于立体视觉系统输入的 立体图像进行在线立体匹配及视差计算，从而构建周围地形图并规划有效安全路 径。此外，火星车还可在线进行基于视觉的运动估计，用以纠正轮式里程计的定 位误差以及惯性导航定位的累积误差。这两部火星车圆满完成了既定的探测任 务，并持续在火星表面工作至今，从而验证了视觉导航定位方法对于移动机器人， 尤其是应用于特殊环境的移动机器人，所具有的重要意义和广阔的应用前景。 纵观该领域的发展动态，各国相关研究机构皆在大力推进相关陆地移动机器 人在自主驾驶、军事侦察、外星球探测、抢险救灾等领域的应用，其中地形环境 的感知与重建、地图构建、基于地图的路径规划、全局定位等关键技术更是吸引 着众多学者和业内人士进行深入研究，尤其基于视觉技术完成上述任务的方法由 于其特有的优点以及相应的技术难度更是成为该领域内的研究热点。因此本文后 续将着重对基于视觉的定位、感知以及地图构建等领域的发展现状进行介绍。 1 2 2 视觉定位技术 视觉定位技术，也称为视觉里程计( v i s u a lo d o m e t r y ) ，主要分为基于单目 或双目立体视觉两种类型。单目视觉里程计【3 】使用设备简单，但需要先验知识且 精度较低，对噪声敏感。双目视觉里程计通常使用立体视觉获得图像特征点的三 维欧式空间坐标并进行运动估计1 4 ，1 5 ，【6 】，【7 1 。其中n i s t e r 等提出了一种可以靠单目 或双目的实时视觉里程计方法1 6 ，采用h a r r i s 算子以及非极大值抑制等方法提取 角点，在局部区域使用饱和值限定角点数目以保证图像上角点的均匀分布，利用 视差约束和一致性约束来减少误匹配，并结合r a n s a c 算法提出了五点法和三 点法运动估计方法，但是基于h a r r i s 算子的所提取的角点特征对图像旋转、尺度 变化等情况的适应性较差，削弱了该方法的鲁棒性。y a n gc h e n g 等设计的立体 视觉里程计7 1 ，在“勇气”号与“机遇”号火星车上得到了成功应用，进一步证 明了视觉定位方法的可行性和有效性；但该方法在一些弱纹理区域无法提取有效 的角点，而仍然需要手工指定对应特征点，此外一些阈值的设定仍然不能有效适 应各种情况，会造成特征匹配及运动估计结果不当。此外还有基于视差空间进行 运动估计的方法【引，基于视差空间的方法具有噪声各向同性分布的特性，能够避 8 浙江大学硕士学位论文 免基于三维空间的运动估计噪声分布不均匀的问题，但是基于视差空间的运动模 型仅适用于小角度转向，实际情况下难以保证运动估计的正确性和鲁棒性。总之， 现有的视觉里程计方法仍然面临特征点匹配、运动估计鲁棒性等方面的挑战，三 维空间中长距离运动估计及全局定位的误差仍有待进一步降低。 1 2 3 视觉感知技术 目前，视觉感知技术已经成为陆地移动机器人进行地形感知与探测的主要技 术手段，具体可分为基于主动视觉和被动视觉两大类别。主动视觉地形感知，如 使用激光雷达进行地形和障碍物检测，已广泛应用于各类移动机器人【l 】，但该方 法成本较高，且对机器人平台体积、功耗等方面要求较高；此外，还有一种使用 结构光的主动视觉感知方法【9 】，但该方法主要适用于室内环境，在室外受各种光 照因素的影响难以保障足够的精度。 另一类基于被动视觉传感器的感知方法通常使用摄像头采集图像，通过图像 处理算法提取有用信息，如深度、结构、纹理信息等。通常使用单目摄像头可以 获得纹理信息，同时可以通过运动获得物体结构信息( s t n l 孤l r e f r o mm o t i o n , s f m ) ，但该方法难以在固定位置一次性获得当前场景全部的深度信息，更无法 真正做到实时深度信息获取。即使基于先验知识可以从单幅图像直接得到深度信 息，该方法的精度以及对于各种地形变化情况的适用性也难以保证。因此，实际 中通常使用基于双目摄像头的立体视觉方法进行三维地形感知与重建【2 】【1 0 1 ，【l l 】。 而立体视觉的核心则在基于立体匹配的视差计算，迄今已有大量各式各样的立体 匹配算法被提出用以解决该问题，但目前综合考虑算法复杂度、精度、鲁棒性等 因素，尚无完全令人满意的解决方案【1 2 】。回顾其中的主要方法，基于全局优化 的方法占有重要地位，如图分割法( g r a p h c u t ) 【1 3 1 和置信度扩张法 ( b e l i e f - p r o p a g a t i o n ) 【1 4 】等。此外，近年来基于局部区域的立体匹配方法也取得 较大进展【1 5 】，但其复杂度，精度、鲁棒性等关键指标仍难以都达到令人满意的 地步。因此，研究相关立体匹配方与视差计算方法是陆地移动机器人视觉导航的 关键之一。 9 浙江大学硕士学位论文 1 2 4 地图构建与通行性判定 用于机器人视觉导航的地图构建方法通常分为基于栅格地图和基于拓扑地 图两类。经典的地图构建及通行性判定方法，如c m u 的m o r p h i n 方法【1 6 1 构建栅 格地图，局部区域使用不同方向弧线上的地图进行通行性判定并选择可行路径， 具体每个方向对应弧线的通行性由该弧线所经每个栅格俯仰角、侧倾角以及粗糙 度加权判定。另一类方法，如j p l 所提出的基于模糊逻辑构建通行性指数，并用 以对不同地形进行分类 1 。此外，还有使用神经网络进行通行性判定的方法【1 8 】 以及应用于室外高速移动机器人的基于概率模型的通行性判定方法【1 9 】。 全局栅格地图通常面临资源消耗随运动范围线性增大的问题，且栅格地图难 以精确反映近距离地形；拓扑地图便于快速搜索规划，但是该方法遗漏了大量有 价值的地形信息，无法描述地图场景中的目标物体或场景，且在大尺度场景中难 以维护。因此，基于栅格的数字高程地图( d i g i t a le l e v a t i o nm a p ，d e m ) 相对而 言可以较为准确的反映地形场景信息并可以极大地减小后续计算负荷，同时基于 地形高度、坡度、粗糙度等参数的通行性判定方法无需过多先验知识，适用于未 知环境和其他复杂环境，相关计算的时间和空间复杂度也较低，因此基于栅格 d e m 的通行性判定为我们提供了一种可行的地图构建及通行性判定方法。 1 2 5 同时定位与地图构建 结合定位与地图构建于一体的同时定位与地图构建问题( s i m u l t a n e o u s l o c a l i z a t i o na n dm a p p i n g ，s l a m ) 近年也成为了移动机器人领域一个单独的研 究方向，其中使用被动视觉方法解决s l a m 问题，功耗远低于使用激光雷达， 而其感知范围远远大于声纳等传感器，因此机器人视觉s l a m 具有明显优势和 广泛的应用前景。目前主要的视觉s l a m 方法主要分为基于图像特征和基于图 像像素两类。基于图像特征的方法通过角点、线段等特征的提取和匹配解决 s l a m 问题，如独立于使用滤波的方法【2 0 1 ，【2 1 】，( 2 2 1 ，基于扩展卡尔曼滤波的方法【2 3 1 ， 基于f a s t s l a m2 0 架构的方法【2 4 1 以及基于光流场的方法【2 5 1 。直接基于图像像素 的方法，如【2 6 1 ，没有考虑运动与结构之间的相关性且对于光照敏感。而常见的 1 0 浙江大学硕士学位论文 视觉s l a m 方法重点在于定位，其地图通常基于稀疏特征点构建，难以完整反 映地形状况并用于导航和障碍物检测。因此，经典s l a m 方法并不能完全满足 机器人视觉导航的需要，我们着重考虑将视觉定位方法与致密地图构建结合，为 机器人视觉导航提供可行方案。 综上所述，机器人视觉定位和地图构建对于机器人视觉导航具有重要意义， 鉴于经典s l a m 方法的局限性，本文采用视觉里程计算法进行定位，并基于该 定位信息将立体视觉获得的致密视差三维信息用于构建全局d e m 地图以实现移 动机器人自主导航。 1 3 三维空间坐标系转换 图1 3 摄像机坐标系与机器人车体坐标系转换关系 视觉感知和重建得到的地形三维点集合通常是基于摄像机坐标系的，而后续 局部地形地图构建通常需要转换到机器人车体坐标系或全局世界坐标系，最终用 于全局导航的全局地图则需要将局部地图转换到世界坐标系。相机坐标系与车体 坐标系间的坐标变换如图1 3 ，其中o c 是摄像机坐标系原点( 通常为立体相机中 左相机的光心) ，乙是延光轴向前方向，坼垂直光轴向左，耳垂直光轴向上， 为摄像机光心相对地面的高度，日m 是摄像机光心和车体中心的高度差，郎是 摄像机光轴- 9 车体坐标系之间的夹角，即相机俯仰角。o v 是车体坐标系原点，乙 为车体前进方向，托垂直车体前进方向水平向左，髟垂直车体前进方向竖直向 上。是摄像机光心和车体中心之间的水平距离。对于本文基于的实验平台， 相机相对车体位置关系固定，即两者是刚体连接，因此相机相对车体的俯仰角 为固定值，可以通过倾斜计等仪器测量得到，此外可以根据相机实际安装情况设 浙江大学硕士学位论文 定摄像机和车体坐标原点在托方向上处在相同位置，且相机相对车体侧倾角为 0 ，或者可忽略不计。相机坐标系到车体坐标系的转换公式如式( 1 1 ) 。 x v 巧 z f l z 妒 _ _ - 一一 10 0c o s 口 0 一s i n 口 o0 0o s i n a h 虻 c o s o f 01 童昂 l l i x c 砭 z c 1 ， ， x w 图1 4 机器人车体坐标系与世界坐标系转换关系 基于机器人车体坐标系的局部地形地图在融合到全局地图时需要重车体坐 标系转换到全局世界坐标系，如图1 4 所示。其中车体当前位置( o ，z w o ) 和姿态( 口，夕，伊) 可以通过本文第2 章的视觉定位算法或使用惯性导航装置 进行航位推算得到。另局部地形地图的高度( 即y 分量) 在融合到全局地图时 需要转换到以参考0 高度为零点的高度值，这里设置机器人车体开始自主导航前 初始位置所在高度为参考0 高度，所有局部地形地图的高度参数以此高度为基 准。通行性判定则依据车体坐标系下的高度值，即地形相对车体当前位置高度的 高度差值，进行判定。车体坐标系到全局世界坐标系的转换公式如式( 1 2 ) 。 1 4 课题技术路线与主要内容 本课题旨在针对陆地移动机器人研究实现完全基于立体视觉的全局定位与 1 2 浙江大学硕士学位论文 致密地图构建方法以及自主导航系统，从而设计实现可行、可靠且实用的移动机 器人自主视觉导航方案。本课题的技术路线为首先分析，比较常用的图像特征点 提取及匹配算法，设计实现基于双目立体视觉的运动估计算法，完成视觉里程计 主体算法；接着实现立体匹配及地形重建算法，在此基础之上完成宽基线立体视 觉方法的研究和实验；最后研究完成基于视觉里程计定位结果与局部地形重建结 果的全局d e m 地图构建方法，并提供相应的通行性判定与路径规划方案。 本文各章节主要内容如下： 第1 章为绪论，主要介绍了移动机器人视觉导航的相关背景，回顾了目前陆 地移动机器人自主导航技术、视觉感知技术、地图构建及通行性判定技术、同时 定位与地图构建技术等相关领域的进展及现状，并在此基础上介绍了本课题的技 术路线以及本文的主要内容。 第2 章主要介绍了双目立体视觉里程计算法的设计实现，具体包括s i f t 图 像旋转、尺度不变特征的提取及匹配、运动估计算法以及视觉里程计整体方案。 第3 章主要介绍了基于立体视觉的地形重建以及地图构建方法。具体提出了 基于联合置信度的改进种子像素扩张方法以及基于宽基线立体视觉的远距离地 形重建方法；还提出了基于栅格d e m 的局部地图构建方法、全局地图融合方法、 通行性判定方法。 第4 章介绍了我们所开发的导航软件以及实验平台，并针对以上各章的算法 方案及整体系统，设计完成了包括室内模拟测试环境和室外自然测试场景的相关 实验，验证了各部分算法和整体导航方案的有效性、鲁棒性。 第5 章对全文进行了总结，提出了对今后研究工作的展望。 本文的主要特色及创新点如下： 设计实现精确、鲁棒、高效的视觉里程计算法； 设计提出基于联合置信度的改进种子像素视差扩张算法； 设计实现基于宽基线立体视觉的远距离地形三维重建方法； 设计实现完全基于视觉定位、视觉感知的致密地图构建方法； 初步实现了较为完整的视觉导航方案。 1 3 浙江大学硕士学位论文 第2 章视觉定位算法研究 机器人自主导航需要以高效、鲁棒、精确的定位算法作为基础。目前主流的 导航定位方法中，基于轮式里程计的定位方法最早得到应用，但该方法在车轮打 滑、空转等情况下难以进行精确定位；而惯性导航装置( i n e r t i a lm e a s u r e m e n tu n i t ， i m u ) 灵敏精确且不易受外界因素干扰，较好的弥补了轮式里程计的不足，然而 该方法在远距离定位中存在显著的误差累积，使精度和可靠性大大降低；而目前 较为常用的基于无线电或g p s 卫星定位的方法不存在误差累计，定位快速准确， 但无法在众多导航信号盲区中使用，如室内、地下、外星球等环境；此外，基 于视觉的导航定位方法正开始广泛运用于各个相关领域。视觉导航定位方法依赖 硬件设备少、对体积、功耗、载荷等要求低，适用于各种复杂未知环境，并能够 填补无线电和g p s 卫星定位的盲区，有效纠正轮式里程计和惯性导航的累计误 差。因此，视觉定位方法越来越成为机器人导航控制领域研究的热点。视觉定位 方法通常需要从每帧输入图像中提取特征，得到相关特征在图像序列帧间的对应 关系，并据此进行运动估计得到帧间的相对位移，最后累加得到每帧对应的全局 位置坐标。本章将对以视觉里程计( v i s u a lo d o m e t r y ，v o ) 为主的视觉定位方 法进行研究，章节2 1 分析了相关图像特征的提取与匹配方法；章节2 2 对常用 的运动估计方法进行了比较分析，章节2 3 介绍了本文使用的视觉定位算法并对 其整体性能进行了分析。 2 1 图像特征提取与匹配 采用视觉方法进行定位，首先需要获取图像尺度与旋转不变的稳定特征并得 到其在连续视频序列中的对应关系。目前常用的特征提取算子，如h a 仃i s 【2 7 】、 f c j r s t n e r 【2 8 】、s i f t t 2 9 1 等都能够提取图像角点特征，其中h a r r i s 算子提取速度综合 较快，但其缺乏旋转不变和尺度不变性，f 6 r s t n e r 性能与h a r r i s 类似，s i f t 算子 对图像旋转、尺度变化以及视角变化鲁棒性最优，但运算时间相对较长，综合考 虑算法性能及复杂度，我们依然选择使用s i f t 算法进行图像特征的提取与匹配。 14 浙江大学硕士学位论文 2 1 1 尺度与旋转不变特征的提取与匹配 d a v i dg l o w e 于2 0 0 4 年提出s i f t ( s c a l ei n v a r i a n tf e a t u r et r a n s f o r i l l ) 算法 2 9 1 ，即尺度不变特征变换算法。该算法是一种具有尺度、旋转以及部分仿射不 变性的局部特征描述算子。s i f t 特征提取包括检测关键点和构造特征描述符两 部分，首先在多尺度空间进行极值点检测，确定关键点的位置和尺度，在其邻域 统计梯度方向直方图并据此构造特征向量，从而实现算子的尺度和旋转不变性。 s i f t 算法的具体流程将在下文中进行简要描述。 s i f t 算法首先通过多尺度高斯滤波构建d o g ( d i f f e r e n c e o f - g a u s s i a n ) 高斯 金字塔，其中d o g 算子为两个不同尺度高斯核的差分，是l o g ( l a p l a c i a n o f - g a u s s i a n ) 算子的简化实现。通过在多尺度空间检测局部极值点，保证了所提取 特征点的独特性和稳定性。其中高斯滤波定义如式( 2 1 ) ，多尺度空间图像可 由图像与高斯滤波函数卷积得到，如式( 2 2 ) ，其中l 表示图像尺度空间，o 为 尺度因子。d o g 算子计算如式( 2 3 ) 及图2 。1 ( 左) 所示。 g ( x ，y ，仃) ：云p - ( x 2 + y 2 ) 2 三( x ，y ，仃) = g ( x ，y ，仃) 幸，( x ，y ) o ( x ，y ，盯) = ( g ( x ，y ，k a ) 一g ( x ，y ，口) ) 木l ( x ，j ，) = 三( 工，y ，k c r ) 一三 ，y ，仃) ( 2 - 1 ) ( 2 - 2 ) ( 2 - 3 ) g a u s s l a ng a u s s i a n ( d o g ) 图2 1d o g 尺度空间构建( 左) 与局部极值检测( 右) 接着计算不同尺度下图像像素的d o g 响应值，搜索其中的局部极值点以及 1 5 浙江大学硕士学位论文 对应尺度。如图2 - 1 ( 右) 所示，检测尺度空间极值时，待检测像素需要与同尺 度8 邻域像素以及相邻尺度的邻域像素进行比较，以确保检测到局部极值像素。 通过三维二次函数拟合确定关键点的位置和尺度，同时剔除低对比度的关键点和 不稳定的边缘响应点，以增强匹配稳定性、提高抗噪声能力。 为保证s i f t 特征的旋转不变性，s i f t 特征向量计算在以关键。d r 、， 勺中心的8 8 窗口中进行。图2 - 2 左图所示区域以关键。d r 、，, 中心，每个栅格代表在该关键 点对应特征尺度上的一个邻域像素，箭头方向代表该像素的梯度方向，箭头长度 代表梯度模值，梯度方向与模值计算如式( 2 - 4 ) 、( 2 5 ) 所示。每个邻域像素 的梯度方向和模值需要根据关键点的梯度方向和模值进行归一化，以实现旋转不 变和对亮度变化的不变性。然后将0 3 6 0 度的梯度方向值域分为8 个区间，在 4 4 领域像素子区域计算8 个区间的梯度方向统计直方图，表示该区域梯度方 向分布情况，如图2 3 右图所示。这种邻域方向信息联合的思想增强了算法抗噪 声的能力，同时对于含有定位误差的特征匹配也提供了较好的容错性。此外为进 一步增强算子的鲁棒性，l o w e 建议每个关键点使用4 4 个上述的领域4 4 像 素子区域的梯度方向直方图来描述其特征，从而得到每个关键点的1 2 8 维s i f t 特征向量。因此，s i f t 特征向量可以适应尺度变化、旋转等情况，通过对该特 征向量归一化，还可进一步增强s i f t 算法对光照变化的适应性。 m ( x ，y ) = ( ( x + 1 ，y ) - l ( x - 1 ，y ) ) 2 + ( 上( 石，j ，+ 1 ) 一l ( x ，y 一1 ) ) 2 ( 2 - 4 ) o ( x ，y ) = t a n 。1 ( l ( x ，少+ 1 ) 一l ( x ，y - 1 ) ) ( l ( x + 1 ，y ) 一l ( x 一1 ，少) ) ( 2 5 ) i7 l ji ? t 乒， tj tl ，弋 ，2 ，、 一一，：lj 一 “；、二气4 | 飞专- o * “w 。j 麓，崞一耐 一一 ，一。二j 。，一， t ；一1_ l ，r ，i - - , 4 i m a g eg r a d i e n t s k e y p o i n td e s c r i p t o r 图2 - 2 基于关键点邻域梯度信息生成s i f t 特征向量 计算得到多幅图像的s i f t 特征后，我们采用关键点特征向量的欧氏距离来 判定两幅图像中关键点的相关性，即对于一幅图像中的某个关键点，找出另一幅 1 6 浙江大学硕士学位论文 图像中与其欧氏距离最近的候选点，如果其中最近邻与次近邻候选点的距离比值 低于阈值，则判定最近邻候选点为对应匹配点。若该比例阀值降低，所得到s i f t 匹配点数目会减少，但匹配准确性会相应提高。欧氏距离最近点的查找使用基于 k d 树的最近邻搜索算法。 k - d 树是- - _ 叉树的扩展，其每一层结点将空间分成两部分。树的根结点按一 维进行划分，下一层子结点按另一维进行划分，并使用类似构建二叉树的方法构 建树。以此类推，各维循环往复，得到一棵类似平衡二叉树。当一个结点中的点 数少于给定最大点数时，划分结束。k - d 树能够降低搜索算法的复杂度，快速高 效的完成特征向量的相关性搜索，并得到两个特征向量的欧氏距离。基于k d 树 的搜索对于低维度空间效率非常高，但对高维度空间其执行效率就会明显的下 降。因此我们使用b b f ( b e s tb i nf i r s t ) 方法对其进行改进，具体通过限制k d 树的最大叶子结点数来缩短搜索时间。该方法可以基于k - d 树快速找到最近邻 特征向量。 2 1 2 特征提取与匹配算法分析比较 为进一步分析比较s i f t 算法的性能，这里我们将s i f t 特征提取与匹配算 法与基于h a r r i s 、f 6 r s t n e r 算子及9 3 一化正交相关( n o r m a l i z e dc r o s sc o r r e l a t i o n ， n c c ) 匹配准则等常用方法进行比较。为了全面比较几种方法提取图像对应匹配 点的性能，我们使用不同尺度、旋转、视角变化的自然场景图像进行测试，具体 对图像序列同帧左右视图中特征点的对应关系、数量、计算时间等进行测试比较， 如图2 3 及表2 1 ，三类算法对于旋转、尺度变化极小的同帧左右图像都能够提 取和匹配出较多的特征点，其中s i f t 算法计算耗时大于其他两种算法。对于图 像序列帧间特征跟踪效果测试，我们使用存在较大的尺度、旋转及视角变化的测 试图像进行测试，如图2 4 、2 5 ，其中s i f t 特征提取与匹配方法可以有效克服 各种尺度、旋转以及视角变化，提取和匹配一定数量的特征点，存在的极少误匹 配点可以通过r a n s a c 方法以及外极线约束去除，从而为后续运动估计提供准 确且足够多的图像特征点对应关系，而其他算法对于此类情况难以进行有效的角 点匹配，得到的匹配结果基本都为错误匹配，会造成后续运动估计的错误结果。 17 浙大学硬士学位论戈 a ) h a t i $ 角点匹配结果 ( c ) s i f t 特征点匹配结果 图2 - 3 模拟自然场景的特征点提取与匹配 表2 1 模拟自然场景的特征点提取与匹配性能比较 h a r r i s f 6 r s t n e rs i f t 左图特征点数 7 21 2 11 1 9 右图特征点数 6 81 2 71 2 8 匹配点数 5 87 3 6 5 堑堕盟! ! ! ! ! ! ! ：型 1 8 斯 学学位论戈 ( a ) h a r r i s 角点匹配结果 ( c ) s i f t 特征点匹配结果 图2 4 模拟自然场景的特征点提取与匹配 浙d 学日士学位豫i a ) h a r r i s 角点匹配结果 ( b ) f j r s t n e r 角点匹配结果 ( c ) s i f t 特征点匹配结果 图2 5 模拟自然场景的特征点提取与匹配 综合分析比较几种常用算法 i a 耐s 算子是广泛使用的角点提取算子，具有 计算简单笄子稳定等特点，对于图像亮度变抉具有一定的鲁棒性然而，在对 角点响应函数执行非极大值抻制从而确定局部极大值时，角点提取的效果很大程 2 0 浙江大学硕士学位论文 度上依赖于阈值，因而对不同图像的适应性有限。f 6 r s t n e r 角点算子可以获得较 高的检测精度，其性能与h a r r i s 算子类似，而其计算量相对h a r r i s 算法较大。s i f t 算法提取图像局部特征并对旋转、尺度缩放、亮度变化保持不变性，对视角变化、 仿射变换、噪声也保持一定程度的稳定性；s i f t 特征向量独特性好，信息量丰 富，能够在弱纹理环境中提取出较多的有效特征点并适用于对大量特征进行快 速、准确的匹配。同时，s i f t 复杂度也远高于h a r r i s 和f 6 r s t n e r 算子。 比较三种特征提取算法对应的特征匹配算法，h a r r i s 和f 6 r s t n e r 特征点常用 基于n c c 的匹配方法，该方法对于旋转、尺度变化的鲁棒性仍有待提高。s i f t 算法采用多尺度邻域的联合方向信息构建高维特征向量，并采用基于k d 树的 近似最近邻搜索算法搜索匹配项i 其虽能够在高维度搜索空间中减少搜索时间并 保证一定的匹配正确率，但总体复杂度较高。 综上所述，基于h a r r i s 和f 6 r s t n e r 算子的特征提取方法主要提取图像角点， 并基于n c c 等准则构建邻域相关信息用以匹配，其算法复杂度低，能够快速提 取大量角点，并能够对平移等简单运动前后图像的角点进行匹配；基于s i f t 算 法的相关方法所提取特征具有对旋转、尺度、亮度变化的不变性，对于视角变化、 弱纹理等复杂情况也具有一定适应性，但是算法复杂度相对较高。综合各种特征 提取与匹配方法的性能优劣，我们仍然选用基于s i f t 算法的特征提取与匹配方 法，以保证后续定位及地图构建算法的精度和鲁棒性。 2 2 运动估计 通过同帧立体图像左右视图中的特征提取与匹配，我们可以得到立体图像中 的对应点，对立体图像进行校正并计算其视差与三维信息，具体图像校正见章节 3 1 ；通过相邻帧特征提取与匹配，我们可以得到帧间的特征对应关系；随后经 过坐标转换我们就可以得到一系列相关特征在车体运动前后的相机坐标系下的 三维坐标。这里我们限定相机与车体为刚体连接，因而相机坐标系下的运动估计 结果等效于车体的运动估计结果。视觉里程计算法在获得前后帧的匹配点对后， 通常在欧式空间中采用奇异值分解进行运动估计。此外，还有基于视差空间的运 动描述方法，该方法具有噪声各向同性分布的特性，能够避免基于三维空间噪声 2 1 浙江大学硕士学位论文 分布不均匀的缺陷，但是基于视差空间的运动模型仅适用于小角度转向，实际情 况下难以保证运动估计的正确性和鲁棒性。因此，我们主要使用基于欧式空间三 维坐标的运动模型进行运动估计。 这里运动估计所要解决的问题，就是已知同一组特征点在坐标系变换前后各 自的三维坐标，从而求出坐标系变换参数的过程。具体其中的特征点通过特征 提取与匹配得到，所求的相邻帧间在相机坐标系下的三维变换参数( 即相机运动 参数或车体运动参数) ，可以用旋转和平移矢量表示，具体如式( 2 - 6 ) 所示。其 中，x d 与砀分别代表当前帧和前一帧中第歹个特征点对应的摄像机坐标系下的 三维坐标。矩阵r 和向量t 分别代表车体的旋转与平移矩阵或向量。r 矩阵如 式( 2 7 ) ；其中缈、口鼢别代表航向、俯仰和侧倾三个姿态欧拉角的角度。 、 瓦= r x p j + t ( 2 - 6 ) f - c o s c o o s f l + s i n f o s i n a s i n f l c o s a s i n , as 洫够c o s + c o s 矽s i l l 口s i i l r = l - c o s c s i n f l + s i n 缈s i n a c o s f lc o s 口c o s s i n c s i n f l + c o s c s i n a c o s f ll ( 2 - 7 ) 【 s i n ( o c o s 口 一s i n a c o s f o c o s 口 j 此外，可以用四元数表示r 矩阵，如q2 q o ，g l ，g ：，9 3j 。使用四元数可以使表 达式无奇点，并能够减少变量，使计算更为简洁和快速。对于给定四元数，仅存 在唯一与之相对应的旋转，用四元数表示的旋转矩阵为： 陆+ g ? 一菇一彰2 ( 明：+ 吼q 3 )2 ( q , q 3 - q 。q ：) i r = l 2 ( q , q 2 - q 。q ，) q ；- q 卜g ；一露2 ( 卿，+ 缃。) i ( 2 8 ) l2 ( g 。g ，+ g 。g ：)2 ( g ：口。- q 。鲕) q ；- q ；- q ；+ g ；i 对每个前后相邻帧进行运动估计可以获得一系列的旋转和平移矩阵，将所有 的旋转和平移矩阵累加起来就可以得到机器人的运动轨迹。下面具体介绍常用的 运动估计方法。 2 2 1 随机抽样一致性算法 高精度的运动估计首先依赖于准确的图像特征对应关系，而实际上即使使用 较为准确可靠的s i f t 特征提取与匹配算法，仍然难以完全消除误匹配点，因此 首先需要在运动估计前有效剔除误匹配点。目前最常用的误匹配点去除方法为随 2 2 浙江大学硕士学位论文 机抽样一致性算法。 随机抽样一致一 生( r a n d o ms a m p l ec o n s e n s u s ，r a n s a c ) g , 法 3 0 1 是计算机视觉 领域内应用最广泛的鲁棒性估计算法之一。该方法并非使用大量数据获得初始 值，而后排除无效数据；而是对数据进行多次随机取样，每次仅随机取出尽可能 少但充分多的数据来确定模型参数，再根据已确定的模型对所有数据进行划分， 在此模型的给定误差范围内的称为内点( i n l i e r s ) ，在误差范围外的称为外点 ( o u t l i e r s ) 。因为外点通常是杂乱无规律的，因此对基于外点确定的模型，内点应 为少数，大部分应为外点。而对基于正确数据所确定的模型应当逼近于真实模型， 因此大部分抽样数据应为内点。经过多次随机取样后，r a n s a c 算法可以找出 使内点比例最高的数据集合，用此数据集合确定最优化模型，从而剔除大部分外 点，保证最终运动估计结果的准确。 2 2 2 运动估计算法 2 1