相关大会1020修改题目动态场景下融合IMU的立体视觉定位

独创性（或创新性）本人所呈交的是本人在导师指导下进行的研究工作及取得的研究成果。尽我所知，除了文中特别加以标注和致谢中所罗列的内容以外，中不包含其他人已经或撰写过的研究成果也不包含为获得邮电大学或其他教育机构的或而使用过的材料与我一同工作的对本做的任何贡献均已在中作了明确的说明并表示了谢意。申请与资料若有不实之处，本人承担一切相关责任。 关 使用的说作者完全了解邮电大学有关保留和使用的规定，即： 工作的知识单位属邮电大学学校有权保 的复印件和磁盘允许被查阅和借阅；学校可以公布的 保存、汇编（的在后遵守此规定注释：本属于在年后适用本书。非论文注释：本不属于范围，适用本书。本人签名 日期导师签名 日期随着机器人技术的发展，机器人将进入的领域，使人们的生活更加便捷。机器人的自主性是使机器人进入领域的关键技术，而精确的定位是自主导航的基础。传统的全球虽然精（1）体视觉定位过程可以分为以下几个步骤：图像，特征提取，立体匹配，特征与运动估计几部分。传统的视觉定位方法，只能坐标系（2）当视觉所使用的场景为动态场景时，由于视位于动态物体上的特征点，提高在动态场景下的定位精息检测到可以有效的减小累计误差，本文实现了基于BoVW的闭环测方法，当机器人回到起始点时，能够检测到闭环最后本文设计的双目立体视觉，通过使用陀螺仪提供进行准确的定位，当机器人回到已经走过的区域时，能够检测到知，视觉定位的精度有所提升：双目立体视觉定位流法环检ResearchonSomeKeytechniquesofstereo-visionlocalizationwithIMUinDynamicEnvironmentWiththedevelopmentofrobotics,robotswillentermoreareasandmakepeople'slifemoreconvenient.ThekeytechnologytomakerobotintomoreareasisautonomyofRobot,andtheprecisepositioningisthebasisofautonomousnavigation.Althoughthetraditionalglobalpositioningsystemhashighaccuracy,ithasreceivedmanyrestrictions.Andtheuseofbinocularstereovisioncangettherobot'sautonomouspositioning,butasinglevisualsensor,canperceivethemovementoftheamountofchange,donotknowtheirownandtherelationshipbetweenthegeodeticcoordinatesystem,whilethegyroscopecangivethegeodeticcoordinatesystem,sothispaperusesthegyroscopeassistedvisualpositioning.Inthispaper,westudiedthe robotautonomouspositioningalgorithm,yzedtheexistingsystemusedtechnical’sadvantagesanddisadvantages.Basedontheaboveresearch、ysisandsomequestionsintheactualapplication,wecarriedoutthefollowingwork:（1）Stereovisionlocalizationprocesscanbedividedintothefollowingsteps:imageacquisition,featureextraction,featurematching,featuretrackingandmotionestimationThetraditionalvisuallocalizationmethodcanonlygetthecamera'schangedmovement,andcan'tknowthetruepositionrelationshipbetweenthelocalizationcoordinatesystemandthecoordinate.Inthispaper,weusesIMUinformationtoassistthevisualpositioning,andprovidesthegeodeticcoordinatesystemforvisionlocalizationsystem.（2）whenthesystemisusedindynamicenvironment,becausevisuallocalizationsystemgetthecamera’slocationthroughthefeaturepointpositionchanged,featurepointwhichonthedynamicobjectwouldaffecttheaccuracyofmotionestimation.Inthispaper,weusingsceneflowdeletethefeaturepointwhichonthedynamicobject,theaccuracyofthissystem. 3）Whentherobotbacktothestartingortheposition,ifrobotcandetectthisinformationthroughvisualinformation,whichcanreducetheaccumulatederror.Thispaperrealizesclosed-loopdetectionmethodthroughbagofvisualwords,whentherobotbacktothestartingposition,oursystemcandetectclosed-loop.Finally,wedesignsabinocularstereovisuallocalizationsystem,whichcanimprovetheaccuracyofthedatainverticaldirection,throughgroundnebygyroscopeprovided,anditcanreturnaccuracyresultindynamicenvironment.Itcandetectclosed-loop,whentherobotbacktothestartingposition.Inthispaper,thelocalizationalgorithmisrealizedinUbuntuoperationsystem,andverificationofthissystemthroughpracticalexperiments.Theaccuracyofvisuallocalizationisimprovedbyyzingtheexperimentaldata.:binocularstereovision scene loop目第一章绪 研究目的和意 常用自的发展现 视觉定位发展与现 选题方向与研究方 第二章双目立体相机与IMU标 2.1机模型与机参 机标定--机参 机标定方 双目测量原 立体标 相机与IMU相对姿态标 标定系统概述与坐标系定 相机与IMU相对姿态标定--标定原 相机与IMU相对姿态标定--标定实 相机与IMU相对姿态标定实验--精度验 本章小 第三章基于场流法动态物体上特征点去除方 运动物体检测方法与光流 场流 场流法原 场流误差模 场流法阈值...........................................................................阈值..........................................................................实验分 场流法在视觉里程计中的应 本章小 第四章基于BoVW的闭环检测方 视觉场景表示方法概 视觉场景建模方法中的BoVW方 BoVW模型的关键技术—词典生 BoVW模型的关键技术—视觉单词选 BoVW方法在双目立体视觉定位过程中的应 实验分 本章小 第五章双目立体视觉.......................................................................双目立体视觉实 系统概 与特征提 立体匹 特征..................................................................................恢复特征点三维位置与运动估 场流法与闭环检 双目立体视觉定位中IMU信息的应 坐标系关 实验分 本章小 第六章总结与展 总 展 参考文 致 攻 期间所获的学术成 第一章研究目的和意移动机器人是一个集环境感知，动态决策与规划，行为控制与执行等多功能于一体的综合系统[1机器人的关键技术之一。根据的应用场景不同，所使用的传感器也有所不同，即相应的与方法也存在着很大差异。定位所使用的传感器可分为以下几类：视觉传感器、惯性测量单元、超声波、激光、全球可以分为绝对和相对。绝对如全球定位系统(Globalnem，)、罗盘等，是指每次定位都是直接根据全局参考信息得到而不需要依赖之前运动过程的[2。这种大多要接收外部信息，在室内等信号缺失的地方无法使用。但其也拥有简便，无累积误差的优点。相对是通过对采样间隔内的运动变化量进行估计，然后对其进行累加，最后，得到当前位姿相对于初始位置的位姿信息的定位技累积误差。在信号缺失的地点相对起着至关重要的作用。不同的有不同的优缺点，因此，一般的都通过融合多种来提高其精度与鲁棒性[2。本文的课题使用惯性导航信息辅助视觉里程计进行定位，同时，使用场流法与基于视觉词典的闭环检测方法，提高其在动态场景下的定位精度及鲁棒性。该技术是一种相对，能够获得全局参考坐标系下的六自由度位姿信息。可以实现对安装有立体相机以及惯性测量单元的移动机器人或用户进行辅助定位，以及为未知环境地图重建提供依据。其平面平行而带来的垂直方向上定位确的问题，双目立体视觉里程计通过场流法去除位于动态物体上的特征点，使该能够在动态场景下正常工常用自的发展现的工作环境，决定了其使用的与传感器类型，也就决定了的特点，如：轮式里程计所使用的编设备简单，视觉传感器成本较低并且可以获得较多的环境信息等。下面对已有的进行分析。全球如图1-1所示，随着的发展，全球得到了广泛的应用。S技术是通过多颗对地面目标距离的探测，以及各间的相对关系，通过几何原理来完成定位[3。S导航系统的是通过测量用户到多颗已知未知的之间的距离，通过综合数据，推算出的位置。经过多年的积累，S导航系统已经发展得比较成熟，且具有很多优点，在，航空等领域已经有很广泛的应用。但是，由于需要外部信号，在含有很大外部干扰或无信号的地方无法使用。1-1GPS激光激光的基本原理是利用光斑将目标位置的变化调制成光斑位置的改变，从而达到对目标进行遥测和非接触式位置测量的目的[4-5]，如图2为激光定位示意图。由于激光测距的精度较高、能力强的优点，在一中高端移动机器人上有较广泛的应用。此外，激光无人车导航及三维图1-2示意图里程计法是一种相对，其原理主要是通过安装在机器人车轮上的1-3是：对机器人的位移增量进行累加，以求取机器人的当前位置。该方法具有使用方便简单，计算量较小的优点，但该方法只能用于轮式机器人且无法获得姿态信息，在短时间内精度较高，在不平坦的路面，受车轮打滑等影响，其定位精度较差。不能够单独用于球形机器人与背负式系统上，但可以作为辅助定位装置。轮轮式码1-3惯性测量单元视觉目前常用的视觉有：单目相机、RGB-D，双目机系统、多目等，如图1-4所示。每种视觉系统具有不同的定位a)单目系 b)RGB-D传感

c)立体视 d)多目系图1-4不同示意单目：单目视觉依靠一个头获取图像序列实现自主定motion法获得真实的尺度信息。单目视觉一般用于室内定位。RGB-D：基于RGB-D传感器是近几年来研究的热点。RGB-D传感器（Kinect）可以同时提供彩色（RGB）与深度信息（Depth息范围有限，且有些传感器存在延时。RGB-D传感器多用于三维重建、视觉SLAM等。双目立体视觉：近年来，随着计算机技术的发展，双目立体视觉定位成为机器人导航技术的研究热点。其中，最典型的应用是发射的1-5所示。双目立体视觉采用的原理是直接模拟人眼观察物体的方式，可靠方便。通过计算左右图像中的特征点在机坐标系中的括：图像，特征点提取，立体匹配，特征点，特征点三维信息的恢复及运动状态估计。双目立体视觉以作为输入，信息量丰富，不需要外界信IMU数据，降低外界影响带来的误差。图1-6火星探测多目：如三目视觉等，也被用于移动机器人的自主定位与地配与的成功率增加。利用多目系统进行三维重建能够提供的空间约多传感器融合不同的特点不同，在一些特定的环境中，往往可以将多传感器融合，提高系统的鲁棒性。如：有研究者研究在室内环境中，使用单目机与内环境中的自主导航任务[6]IMU数据辅助视觉定在视觉里程计与惯性测量单元融合过程中，不同的应用场景可以采用不同的数据耦合和方法。根据融合系统中信息耦合的程度，.Corke[1在其文章中将融合系统大致分为两类，一类是松耦合，一类是紧耦合[2为惯导与视觉两个模块，他们分别以不同的速率计算，可以将每组数据添加上时间戳，将时间最相近的运算结果进行融合。由于松耦合系统不考虑传感器之间的关系，故有利于各个传感器数据的优化。紧耦合系统一般考虑传感器之间的耦合关系，将不同传感器数据放在一个滤波器中进行优化。但这种耦合方法一般不利于处理较大的越变信号。无论传感器数据之间属于哪种耦合关系，大部分数据耦合方法都是基于卡尔曼滤波器的。面对已有的视觉惯导系统进行简要介绍。KurtKonolige[8]等使用拓展卡尔曼滤波器将惯性测量单元与视觉里程计数据据做观测，视觉里程计数据做预测。TaragayOskiper[9]等使用拓展卡尔曼滤波器将多个双目立体相机与惯性测量单元数据进行融合，假设角速度与速度是不会突变的定值，并通过对视觉里程计与惯导的噪声分析选择滤波器管则更新所使用的数据。thanlly[7等主要研究无人机定位，其方法是将视觉与惯导的信息通过拓展卡尔曼滤波器进行融合，滤波器的状态矢量除了包含位移、速度、姿态航向外还包含陀螺仪零漂、加速度计零漂、重力加速度等信息。光照调节，质量等因素的影响。可以看出惯性测量单元与双目立体视觉有IMU数据与双目立体视觉相融合，提高视觉定位发展与现立体视觉定位最先是由ec[12]，他说明了如何使用视觉信息测量机器人的移动距离，为视觉定位拉开帷幕。随后，ttieshafer等提出了立体视觉定位的计算框架，主要可以归纳为以下几个部分：图像、特征点提取、特征点匹配/，运动估计。通过实验验证了该框架的可行性并在室内短距离的试验中取得了良好的效果。然而，航空航天局喷气推进（JPL）SSTB-Lite漫游者平台，并通过Nister依靠单目或者双目视觉[17]，对图像提取Harris角点并对所提取的特征点进行，使用随机采样一致性算法在运动估计过程中，去除局外点，以提高运动估计的精度。Konolige等[18]BundleAdjustmentBASLAM中常用的算法之一。Lowe等提出了具有良好的尺度不变性与旋SIFT特征点的计算较复杂。避免的带来了累计误差。有效地利用场景信息可以减小累积误差，通过视觉信LM的主要思想。通过场景特征的全局LM相对于视觉里程计的一个优点，但在提高精度的同时也增加了计算量。ithfcheesan21M的概念，想达到的目标是：当机器人工作在未知环境中时，通过视觉信息对环境进行描述，并构建地图，并通过地图与环境中的特征实现自主定位。SLAM领域，定位与地图是完全耦合的，实现定位，就需要知道环境中的特息，而构建地图的基础是知道当前位置信息。由于环境信息和位置信SLAMSLAM、RGB-DSLAMSLAM。早期SLAMSLAM，FastSLAM，当前比较流行的SLAMBAORB-SLAM。 选题方向与研究方IMUSLAM中常见的基于视觉词典的闭环检测方法，结合实际应用需求提出了一种新颖的、可用于移动机器人，背负式系统的视觉。本器人、已有视觉以及立体视觉的发展应用现状，阐第二章中主要介绍了本文所使用的相机模型、相机的标定原理、双目立体IMUIMU涉及的关键问题进行了分析并对标定实验进行了总结，使用两种方法对标定结IMUIMU辅助视觉定位奠定了基础。觉能够工作在动态场景下，提供了条件。本章还介绍了动态物体检测方法，对不同的方法进行了分析，总结出适合双目立体视觉的动态物用的闭环检测方法与关键技术。然后，对词典的构建步骤与相似度检测方第五章对本文设计的视觉进行了详细介绍，本系统可分为：数据，特征提取，特征匹配与，特征点筛选，运动估计，闭环检测，IMUIMU信息辅助视觉定位。最后，通过实验验证了本文所设计的定位问题，对文中双目立体视觉进行了总结，并对后续研究的方向第二章IMU对IMU信息的应用为提供大地平面，提高垂直方向上定位数据的可用BoVW的闭环检测方法，实现闭环检测以减IMU信息辅助双目立IMU相对姿态进行标定，本章2.1机模型与机参括内参数与外参数，其中内参数主要包括：主点坐标(u0v0)f。相机内3D2D像素点之间的联系。由于任何两个坐标系之间的相对使用双目立体相机与卡获取图像信息进行定位，故需要对双目立体相机进IMUIMU态进行标定。 本文所实现的视觉所使用的机的模型为最简单的针孔模型，2-1可以看出，相机前方的物体在像平面上所占区域的大小与焦距有关。2-1以机的光心为原点，与光轴平行的方向为Z轴，以相机到景物的方向Z轴正方向建立坐标系，X，YX轴正PX,YZ，在成像平面上的投影点p(x,y,f，按照相似三角形原理，在相机坐标系下空间点与其像点之间有如xcx y f

fx是透镜的物理焦距长度与成像仪每个单元尺寸sx的成积，fx的单位是像素。 机标定--机参空间一点P Z，映射到投影平面上坐标为p

投影变换。现用一个三维向量q q3来表示像平面上一点，同时除q3就得到了平面上一点坐标。根据2-1所示，物理世界中的点投影到机x cx Xq=MQ，其中q=y,M c,QY

y 1 它们分别来源于透镜的形状与机的组装过程。本文指简单介绍径向畸变畸变比较小[44]，r0位置周围级数的前几项可以用来描述径向畸变，一般uu(u'u)(1k'r2 vv(v'v)(1k'r2

(uv')(uv)k'k'：u，v 标系中一点Po

zo在相机坐标系中坐标为Pc

zcPcR(PoTR3*3的由世界坐标系到相机坐标系的旋转矩阵，T3*1的平移向量。 机标定方机标定是为了准确地获得相机的内参数与外参数，从而确定世界坐标系{W}与机坐标系{C、以及世界坐标系{Ｗ}与图像坐标系{I}之间的一一应关系，是双目视觉定位中一个重要的步骤40。目前已有许多学者对相机标定方法做出了研究，立体视觉中常用的标定方法有传统标定法、主动视觉标定法与自标定法。相机自标定法不需要特定的标定标靶，根据所采用的方法不同，可以分为基于场景特征的自标定和基于运动的自标定。主动视觉标定法是指“已知机的某些运动信息”下标定相机的。该方法能够对机进行标定的前提条件是：已知 机的运动信息。传统标定法主要通过对特定的标定标靶进行图像处理，根据一系列代数变换计算求解相机的参数。f，xfxsxf，yfysyf，光心的图像坐标 cy；在双目立体视觉定位中涉及到的重要坐标系有：世界坐标系，机坐标相相机坐标y像平面坐标xouxvy图像坐标P世界坐标（2）机坐标系{OC相机坐标系是以相机光轴中心点为坐标原点建立的，Z轴平行于光轴且垂直X轴方向所建立的右手坐标系。相机的外参像平面坐标系{x-机光轴与成像平面的交点为坐标系原点，X轴方向为图像横坐标方向的坐标图像坐标系{u-x

u0dxuy y

v0dyv1

1 双目立体视觉模拟人类双眼的工作方式，通过同一场景在不同视角下的观察图像[44，恢复出深度信息。如果已知了机的内外参数，就可以建立3D彼此的关系。PZrpcrightPZrpcrightxffTor现假设两台机前向平行排列，即两幅图像是行对准的。现有 Pppxlxr。现定义视差定义为dxlxr2-3所示，深度与视差成反比。利用相似三角形原理 T(xlxrZ

TZ b* (xlxr

2-3变化只能引起很小的视差变化，也就是说，立体视觉系统对于距离机较近的物体具有较高的精度。由2-6与相似三角形的原理，可以得到相机坐标系下空间点P的三维坐 xb

0 b

dXc d

x l

0

y

c

y Zc

1zb

1

d 1 根据2-5和2-7可以得出世界坐标系与图像坐标系之间的转换关系ddXw

0

dxu0Y 00

w=

v0dyvZw

fb

11

d 1 物理空间中两个机相关位姿信息可以通过本征矩阵表示。而基础矩阵机的内参数[44]E是单几何意义上的，与成像仪无关他将左立体标定主要是求解两台机之间的位姿关系，其中包括：先转矩阵与平移向量T，如图2-4所示。立体标定与单机标定的不同是单机标Px,y,z RT2-4给定空间中一点P我们可以分别用单机标定来将点P输入到左 两个视图可以用PRT(PT)关联，其中R和T分别为两个机之间的旋转 RRr(RlTTr

阵（Rr、Rl）和平移向量（Tr、Tl，然后将求取结果带入2-9中，就可以为了更加准确的计算视差，使两个像平面完全对准是必要的（5-52-45-5是：对两台机的图像平面重投影，使得他们精确的落到同一平面上，而且图像的行完全的对准到前向平行的结构上。相机与IMU相对姿态标IMU信息来辅助视觉定位，首先需要对相机与陀螺仪进行位姿标2-5IMU组成。IMU与相机固连在一起，两传感器处于相对静止的状态。实验过程中，细的重力方向向量。静止情况下，IMUCIMUIMU坐标系与像机坐标系之间IRIMUCIgIRCgIgCgIR 机与IMU固连单元以不同的姿态，静止于细线前方，多几组数据，提高标支架支架2-52-6所示，本文中主要涉及三个坐标系，分别为：相机坐标系，IMUy C z R R x2-6相机坐标系：{C}2-7示，相机坐标系以左相机光心为坐标原点，X，Y轴与像平面平行，Z轴与相机yxP(yxP(X,Y,ZpO主点(cczf主光图像平2-7IMU坐标系：{I}IMU坐标系，如2-8IMU重心为坐标系原点，X，YIMU平行，ZIMUIMU上方。2-8IMU大地坐标系：{G}X轴指向磁北极，Z轴IMU相对姿态标定--文中采用细线与重物确定相机坐标系下的重力方向，在重物和细线保持静2-9l,ul,v，同样也可以在右图像上找出相应的点。根据双目测距原理，可以求解出细线上的两点在相机坐标系下的三维坐标。故可2-10p1p2IMUb通常惯性测量单元的加速度计测得的加速度ag和系统自身加速度aagaIMUb为重力加速度，如2-11所示，将IMU所测得的加速度信息单位化后，即得IMU坐标系下的表示。 11)IMUIMU返回信息不能直接使用，本文采用一种比较简便的方法，以系统时间作为参考，记录下每组的时间戳与IMU返回每组数据的时间戳，根据每组的时间戳寻找与其相对时间差最小的IMU数据。由于时间是线性增加的，在IMU先于相机数据的情况下，理论上每组都能够找到与之对应的IMU数据。IMU相对姿态标定-- Ig为第iIMUCg为第i次测得的重力方向在相机坐标系下的单位向量。现假设四元数q表示从{I}系到 o 系的位姿变换，于是，有CgqIgq，根据参考文献 n o 法可知：能够使(qIgqCg取得最大值的旋转四元数q

i编写程序驱动相机与陀螺仪程序，设定一对左右同时25IMU启动IMU－Camera单元加速度和实验多组完整数据，从中选出20组数据，进行后续计算，求解位姿变由标定原理中相机坐标系{C}IMU坐标系{I}下重力方向向量计算方在实验过程中，每组数据时相机-IMU固连单元应保持静止状态，是被IMUIMU系下的表示。另外在各组数据时要求相机-IMU固连单元的位姿相互独立，同时保证IMU相对姿态标定实验--标定完成后即可得到相机坐标系{C}IMU坐标系{I}之间的旋转矩阵与旋180.4772-10所示：21018 0.0039R IIMU返回的三轴加速度、三轴转弯速率（速率陀螺）和三轴地球磁场数IMU坐标系到大地坐标系下角度（俯仰角，滚动角，偏航角IMU坐标系到大地坐标系下的旋转矩阵GR。将相机坐标系所测得的重力方向向量Cg转换到大地坐标系I

GRIRC

GRIMUIRIMU gcg当前位置的重力方向向量之间的夹角，也可以评估标定误差。分别一百五十组与一百三十组左右数据，转换到大地坐标系下，如图2-11与图2-11所示，均误差分别为0.487度于0.401度。且最大误差小于0.54度，由于IMU测得加速度误差为0.5度左右，所以可以看出本次标定实验精度2-112-12本章小本章主要介绍了本文所使用的相机模型-针孔模型、机标定的原理、双IMU相对姿态标定原理。其中，重点介IMU相对姿态标定原理与步骤，通过实验完IMU相对姿态标定，并且对实验中的每一个步骤进行了详运动物体检测方法与光流fer和mn所混合模型。其主要的思想是：通过不同的分布对不同的背景像素进行背景建模，这样在背景变化较大的情况下，此模型也可以表现出良好的效果。但是，对于缓慢的运动目标，即背景变化很小的情况下，此模型的效果不是很理想。由于背景减除法比较消耗内存，并且这种方法通常是假定机位置姿态固定不变的情况下使用的，视觉多位于移动平台上，机的运动会在视觉中，背景减除法并不是最适合检测动态物体的方法。的重视，图像理解的问题是目标检测与分割。一般用于检测出某一类物提取方法对进行特征提取后，在分类器进行训练，也就是对待检测物体建待检 进行特征提取。然后根据训练好的模型对待检 进行分类。前，图像理解的方法在运动物体检测方面达到了较好的效果。常用的图像理解M，BoostingM（支持向量机）分类器进行行人检测的实验结果。Boosting90%以上[40。3-1SVM如果使用图像理解的方法检测动态物体，可以再图像完成后，先对图这种方将静止的车辆与行人也排除掉，即一些局内点也被去除。当车辆或行人距离较远，即这部分区域在整幅图像中所占比例不大，去除后，对定位精度影响不大，但当车辆或行人距离较近并且静止时，车辆上[44]，主流的方法是计算稠密光流的速度场，但能够计算像素点运动速度的前是，在下一帧中找到与上一帧中对应的像素点，故计算光流的前提是成功的跟踪被计算的像素点，如图3-2为对室内行人进行光流检测的结果。所以，在纹不丰富的情况下，计算稠密光流是很的。例如：当我们看到一张运动的白纸时，位于白纸中的像素点在连续几帧中一直是白色的，即无法计算像素点的移动速度。故只能计算位于白纸边缘的像素点的移动速度。一般稠密光流的计算是根据已知运动速度的像素点使用某种差值方法，计算不容易 的某些像于是有另法：稀疏光流。与稠密光流不同的是，稀疏光流主要计算一些具有明显特征的像素点，如：斑点，角点等。这些像素点的会更稳征的像素点的运动，故当中存在缓慢移动的物体或背景发生变化时，仍然影响，很难提取出完整的物体轮廓。可以用来直接识别运动状态与机运动3-2场流理论上，运动场可以由每一个运动的像素点描述，当机或目标发生运 t图像平面如图3-1所示，中P为空间点P在成像平面上的投影，当点P在空间中运动矢量为t时，反映在图像平面上的对应变化为t。可见光流的变化在一定程度光流法中，认为图像是符合瞬间灰度不变假设的，设图像点(x,yt时刻的灰度为I(x,y,t)，设该点光流在水平和竖直方向的分量分别为u(x,y)、v(x,y)。经tI(u+u,v+v,t+t)，当dt趋向于零时，可以认为各个像素点的灰度值I是保持不变的，将I(u+u,v+v,t+t)进行展开（只保留根据前面的假设条件灰度值保持不变，得I(xx,yy,tt)I(x,y,t)，可IxIyIt0，两边同时对t求导有： IxIyIt0

I

I

I0故可以得 x

y

t

x

y 2）该方程被称为光流方程，由于一个方程中包含了两个未知量Ix，Iy，故单个像LK（Lucas-Kanada）算法是借助于点周围的小窗口信息来求取光流，即通 此外，LK算法有三个假设条件，分别为：亮度恒定、时间连续、空间一致，其表达得和3-2表达得基本一致。通过LK算法可以求取出点的光流（1）通过R和T分别表示机器人的旋转与平移情况，相邻两帧图像中的特征点Xt uu0b/dYvvb/dt Z

fb/d t Xt+1

uu0b/d Rvvb/dt+1

fb/ t+1 t时刻特征点的三维信息如3-3所示，3-4为通过已知的相机运动参数将t时刻特征点的三维坐标投影到下一时刻。将根据3-4求解出的特的运动规律是否与相机的运动规律一致，计算方法如3-5所示。理论情况M值应接近于零，但若该点为动态点，即与相机的运动规律不一致（发生了相对运动M值应大于零。tt

u'u

uu YMY

-

t1

v'v

R

v

T0l0t0l0

t1

u'u'

0 u

0 Z

l

t

t1

在理想情况下，3-5能够计算出没一个特征点是否属于动态点，但是由于成像误差，噪声等原因，使得计算结果并不一定十分准确。距离机越远3-4所示。

图3-4三中一点到图像平面成像误差示意zzbd

1110 像素点在图像平面的位置符合分布，均值为其理论投影位置，左右图像在水平方向与竖直方向的投影误差2=2=2=1。根据空间点位置计算(3-3)，计算其比矩阵，即对等式右端的三项分别求ul,ur,v的偏导数。其结果3*3 (b*(uu))/d (b*(vv)/d b/d2 J(b*(uu))/d (b*(vv)/d b/ 差表达式为diag(2,2, x3*3矩阵，矩阵对角线上的元素表示该点在某一方MTx马氏距离描述的是在统计学中服从统一分布的随机样本差异度。马氏距离越大，则对应的点越有可能属于动态物体，通过相邻两帧图像就可以求取特征点的马氏距离，本文所采用的马氏距离的计算如MTx

场流法阈值阈值场流法通过马氏距离的大小筛选出位于动态物体上的特征点，但马氏距离的计算是建立在总体样本的基础上的，其含义可以解释为：单个样本与总体样本的差异程度。但同一样本放到不同的总体中会呈现出不同的马氏距离的值。例如：行走速度为1m/s自行车上的特征点的马氏距离在移动速度为3m/s移速度为5m/s的移动机器人的视野中会呈现出不同的值，且总体样本也会呈现不同的马氏距离的值。然而，当视觉 协助机器人或背负式系统的完成特速移动的物体又包含移动速度较慢物体，故固定的阈值并不适合视觉3-6所示，横坐标表示马氏距离的值，纵坐标表示处于这一范围的特征点的数3-64.9点，使用不同的阈值进行检测。以下分别进行两组实验：实验一：对位于车上的特征点进行检如图3-6为当前中特征点所对应的马氏距离的直方图统计结果。图3-8为使用两种方法所得出的特征点筛选结果，其中，红色点（使用正方形圈出的点）3-83-8使用本文的方法筛选0 实验二：对位于行人上的特征点进行检相对于公的小汽车而言，行人的移动速度较慢，理论上马氏距离的值应该分布在较小的范围之内，如图3-6，图3-6，图3-6分别为对中特征点检测结果。通过上述实验可以得到以下结论：基于直方图统计的阈值0 3-93-113-60503-903.5马氏距离的分布范围较大，在相对运动较小的场景中，其分布范围也较小，故在不同的环境中，马氏距离的值的分布是不一样的，固定阈值只能够适应某种特殊情况，而不能满足所应用的所有的情况。从图3-6可知，在动态体的移动速度较快的场景中（如：高速公，特征点的马氏距离分布范围大，且样本整体的方差较大，固定阈值在此类场景中能够筛选动态点，但会多3-9体的移动速度较慢的场景中（如：室内或行人较多场景中，特征点的马氏距离分布范围较小，相对而言，选择出合适的阈值的难度较大，此时，固定阈值在此类场景中仅仅能够筛选出很少的动态点，此时，动态的阈值能够筛选出位于动态物体上的特征点，虽然也包含了一些静态点，但这已能够满足的基本应用场景。场流法在视觉里程计中的应在动态场景下，对场流法进行试验验证，实验在邮电大学校园内进3503-103-11是位在动态3-10中可以看出，X，Y方向上定位结果虽有提高，但提高并不是十分明显，而在垂0.250.05米左右，定位结果有了明显提高。经RANSAC祛除了一部分离群点。故在动态物体较少的情况下，X，Y，Z三个方向上定位结果均有所提高。zz03-10（x-x-503-11（x-本章小本章主要针对视觉工作在动态场景下会遇到的实际问题进行了研的分析，对马氏距离阈值进行了改进。最终，使视觉可以在BoVWSLAM的基础问题之一，在视觉定位过程中，由于运动估计与词典的闭环检测方法是用视觉词袋模型BoVW表征图像内容，通过相似性视觉场景表示方法概看，图像中的景物在空间中是按照某一集合规律构成的。的表示可以采用不同的方式，但其目的只有一个，即用低维的特征表示的图像，方便计算机的与计算。现。Nourbakhsh[33]Lamon等[29]分别使用一系列直方图与有序的边缘颜色特征描述场景外观。Krose等[30]Ramos等[32]分别使用主成分分析方法与维数约简、变分学习方法相结合降低图像的维数。现有的大部分方法的思路都征是至关重要的。LiKosecka[32]选择具有良好不变性的局部特征进行场景识降低[35]。由于离散化的特征有利于计算机的，将图像抽象为离散化的点的表示是近年来常用的方法之一。这种方法的思想主要来源于文本处理中比较成技术，将图像信息使用一个标准的离散化的单词库中的单词集合表示，用相同维数的向量描述不同的，如图4-1所示。这种方法即使用图像层特征来表征图像内容，也利用了离散化单词方便计算的优点，同时还建立了统一的规范化的描述框架，已在机器人视觉导航中取得广泛应用[34。图像表将Bag-of-words模型图像表从图像中提取出相互独立的视觉词每幅图像的直方图表4-1视觉场景建模方法中的BoVW方BoVW模型的关键技术—为机器视觉领域提供了新的思路。其主要思想是把的图像特征，投影到离散化的低。BoVW中构造词典的主要步骤可以总结为：对图像提取底层特征，如：SIFT，ORB在进行图像分类或图像相似度判断时，可将待检测投影到词典空间，得到每一个视觉单词在待检测中出现的次数，通过统计结果完成对进行分类或相似度判断等任务。图4-1是基于BoVW的目标识别的一个实例。该基于BoVWBoVW造词典的环节。词典的大小决定了，图像投影到空间的维数与相似性计算的复杂度，是对于无任何先验知识的实际问题，仍无有效的方法准确的确定词典大小[35]了计算复杂度，也影响了视觉词典的表征性能，如何去除冗余信息，保典BoVW的关键技术之一。BoVW模型的关键技术—的特征包括：SIFT特征，SURFORB特征，这三种特征点均对尺度与SIFTSIFTDavidLowe19992004年完善的局部特征描述子，SIFT是具有良好不变性的局部特征，其对旋转变化，尺度缩放、亮度变4-2是对图像SIFT500左右。4-2SIFTSIFTstep2.为使其具有旋转不变性，需计算关键点邻域像素的梯度方向。SIFT特征的主方向是由关键点邻域内的梯度方向分布决定的。特征点(x,y处的梯度和模值如所示：(L(x1,y)(L(x1,y)L(x1,y))2(L(x,y1)L(x,y(x,y)atan2(L(x,y1)L(x,y1),L(x1,y)L(x1,

m(x,y为模值，(x,yLL(x1,y)为在某一尺度下，(x1,y)处像素点与相应的核卷积后的值完成关键点的梯度与模值的计算后，使用直方图统计邻域内像素的梯度和0~360度的方向范围分为若干个柱（bin向代表了关键点的主方向。step3.SIFT16*16的邻域作为采样窗binbin8位，故描述符为：4*4*84-3所示。KeypointImage4-3SURF2006HerbertBaySURF特征提取算法。SURF特征是对SIFT特征的一种改进，SURF特征也是对尺度变化、旋转变化、角度变化以及SIFT，SURF在计算速度方面有很大4-4SURF特征提取示意图，提取到的特征点数量为800左右。4-4SURFSURFstep1.特征点检测：SURFhessian矩阵的，hessian矩阵是图像的二阶导数。hessian矩阵的行列式的局部极大值与极小值代表了特征step2.尺度空间的模拟：函数适合模拟尺度空间，但必须被离散化与剪裁，SURF特征提取过程中，使用盒子滤波器来近似二阶偏导，对图像进行step3.主方向确定：大部分特征都是通过赋予特征点方向信息，达到旋转不变性。以某个特征点为圆心，在其圆形邻域内，计算两个方向上的Haar响应，Haar的模板如图4-5所示，图中，白色表示+1，黑色表示-1。即Haarx，y方向的特征值应为相应的白色部分像素值和减去黑色部分像素值。X方向响 Y方向响图4-5haar滤波器示意接着，以特征点为中心，圆心角为/3Haar小波响应值dx、dy的累加，每个窗口所有点的响应之和构成一个矢量，如（4-2）step4.20S*20S分为4*4个子块，每个子块用尺寸2S的Haar模板进行x,y两个方向上响应值计算，并统计每个子块中的dx、dy、|dx|、|dy|，这样就有4*4*4644-6 |d||d|ddyORB201ICCV会议上，EthnRublee等[13B特征检测算子，B(orietedTandroatedRIEF)TBIEF特征描述符的基础之上的。T角点检测的主要思想是通过对像素点周围邻域内的像素点存在较大的差异，如：某像素点周围邻域内有足够多的像素点的灰BIEF描述的主要思想也是通B特征描述符十二进制的，能减少特征与立体匹配过程中的计算量。但是由于单一的T角点测、BIEF描述算法不具备旋转不变性，以及尺度不变性。并且对噪声比较敏感。因此，BBIEFT思想相结合，以对其缺点进行改4-7B特征提取的示意图，提取到的特征点数量为500左右。4-7ORBORBprnIk

n

Ik为圆周围第kIpp的灰度值。t为一个比较小的阈值。当s1的阈值的个数大于一定的值时就认为该点为特征点。step2.T特征点的基本步骤，但是不具有尺度不变性，因此使用常见的模拟尺度变化的方法，建立图像金字塔，在每一层中按照T性，加入了灰度质心的思想，来确定特征点的主方向，是通过计算特征点的矩来得到特征点的方向的。图像中某特征点淋浴内的pq阶矩的定义式为则特征点质心坐标(cxcy)step3.特征描述符生成：ORBBRIEF描述算子的。但添加了旋转不变性。BRIEF描述算子的每一位是通过随机选取的两个像素点比较得来的，假设一个经过平滑后的图像块为p的比较准则为npxxpyy处的灰度值，选择n对像素点进行比较，就得到了n*1fp2i1pxyn符。BRIEF描述算子为一个二进制描述符，从可以看出，BRIEF描述算子为了提高BRIEF描述算子对噪声与旋转的鲁棒性，ORB特征利用函数对图像进行滤波处理，并且将像素点的比较改为小图像块（5*5的像素块）BRIEF描述算子对旋转敏感的问题，ORB特征也做出了相应FAST特征点的主方向。对于任意的特征点的集合，在一点(xi,yi处的n维二进制准则为，定义一个2*n的矩阵：xnyxnyns

使用由关键点周围邻域图像求解出的方向R构成ssRs。此时，BRIEF30个

302π/30次关键点计算的s来生成描述符。另一种通过贪婪穷举的搜索方法找到相关系0.5的像素点对。通过特征点提取数量可以看出，SIFTORB特征提取的特征点数量略SURF特征，并且从耗时来看，ORBSURFSIFTSURF特征表征BoVW随着计算机视觉在领域的应用，越来越多的研究者关注BoVW方法，同时也取得了很多成果。例如：Angeli等[38]同时使用形状和颜色特征构建词典，以满足机器人闭环检测等实际问题。Wang等[37]使用算子，对图像进行特征提取并生成字典本。在对图像进行BoVW描述后，在对BoVW进行匹本课题中采用的词典生成方法来源于Wang等[37]的思路，先对场景内所有surf特征点并描述，再通过无监督聚类（K-means）K4-8如图4-8中右图为计算每幅的BoVW描述符，以判断相似度，为视觉进行闭环检测提供依据。BoVW描述符的计算过程主要为：A:对当前帧提取surf特征点并描C:统计当前中所有单词出现的次数，形成一个K*1的向量，即为每幅BoVW描述符。D:计算不同BoVW描述符间的欧氏距离，确定相似度有上述可知，视觉词典的优劣直接影响相似度的检测效果，近而影响计算bow（相似度判断对每一对每一 提取特征（surf），生成描述 将“单词”以固定的格（词典 BoVW在双目立体视觉中，分为两个线程，一个线程执行视觉里程计，4-9所示。A:20帧不进行位姿判断，20帧之后开始进行位B:当前位姿与之前位姿小于一定阈值时，进行相似度判断C:当相似度小于一定阈值时，即为出现闭环，继续观察D:当连续出现相似性与位姿均小于一定阈值时，及确认出现闭环闭环检测线得得到当前位相相似度大于阈值，即为闭环候选计算记录下 与当 的相似 当前已处理帧数大于20时开始进行，开曲曲线拟4-9实验分本文通过对邮电大学校园内多张图像，对所有图像提取surf特征10000个视觉单词的词典。通过与词典中单词进行匹配，判断相似度，如图4-10所示，图（a（b片（b（c）与（d）与（a）BoVW特征描述符间的欧氏距离，其的结果分别为：0.32，0.36，0.41，从结果中可以看出（b）与图像（a）最相似，即BoVW方法可以有效的计算相似度，能够满足闭环检测的需求。

图4-10相似度计

本章小现有研究表明bag-of-words方法在检索，分类领域已经得到了广泛BoVW基本原理，应用BoVW的相似性判断与基于BoVW的闭环检测在双目立体视觉中在自主中，需要利用传感器感知周围环境信息，检测自身位置与IMU信息在双目立体视觉中的应用。双目立体视觉实双目立体视觉主要由：图像，特征提取，立体匹配，特征跟踪，运动估计，基于场流法的特征筛选，闭环检测，IMU信息应用几部分组5-1sobel算SIFT特征点的描述方法进行描述，氏距离进行特征点，利用前后两帧图像的三维特征点的信息，使用牛 加 加速姿角速左图双目相 特征点提特征（左图像立体匹运特征点筛（场流法运右特征点提（类似于算法特征（右图像特征提特征匹配运动估计特征点提与词典中的单进行匹配描述符当位置信息与间的距离小于阈检测到闭模闭环检测图5-1双目立体视觉结构框i同时启动双目立体相机与惯性测量单元，用已标定的双目立体相机与惯性测量单元固连单元进行连续，如图5-2所示。其中，本课题中使用所使用立tyBumblebeeB2-03S2C数字机，试验中所设置的帧率为30FP，图像分辨率为640*480。使用的i数字陀螺仪，其中集成了三轴磁力仪，三轴加速度计，三轴陀螺仪，其内部低功耗信号处理器能够提供无漂移的三维方向以及校准的三轴加速度、三轴转弯速率（速率陀螺）和三轴地球磁场数据。实验时，惯性测量单元输出实IMU坐标系到大地坐标系下的旋转矩阵GR，使用iRS-232串口IMU数据，波特率设置为1152005-2（2）在计算机内存中，将图像（灰度图）个位置的值的物理意义是像素值。图像的特征点一般具有良好的不变性，如：尺度不变性和旋转不变性，本课题中提取的特征点包括：交点，斑点等。本课l算子分别对做右图像进行边缘提取，使用水平方向与竖直方向上的核函数与图像进行卷积，将两边缘的交点作为特征点的候选点。l算子通常有两个，各自独立地检测水平方向和竖直方向的边缘，常用的卷积核为： 1G 0

G

0sobel3步step1.3x3的卷积核Gxx方向Dx(x,y)；step2.3x3的卷积核Gyy方向Dy(x,y)；step3.xy(D(x,y))2(D(x,y))2(D(x,xy

5-3blob算子生成其特5*5范围内，分别选取四个角落上的四个点，对每816*8位。后续通过描述符进行立体匹配。提取角点之后还要进行一次筛选，以上的特征点作为中心，在其邻域内寻找，去掉距离这些特征点较近的点，是特征点分布均匀。计算特征描述符间的相似性，在右图像中找出左图像中的特征点对应的位置。通过计算视差，得到点的三维信息。其中，立体匹配算法的准确性受到多种因sobel算子提取出图像角点之后，对所提取的每一个交点，使用其128维的特征描述符，通过计算同一纵坐标上特视觉系统光轴平行，因此根据极线约束可知，对应左右特征点的纵坐标5-4了大量的深度信息，故一般需要约束条件才能提高匹配结果的准确性，下面是一些最基本约束条件42。PPx,y,z主光(c,c(c,cfOxTyO5-5Px,yzPx,yz在左右图像平plpr，成为共轭点。设Ol、Or分别为相机中心，即为相机Px,yz与左、右相机中心构成的平面成为外极平面。plpr也应该是连续的，但在同一物体表面，物体边界的深度是不连续。此约束在遮挡区域中此约束不成立。上的特征点，理论上灰度值应该具有一致性。另一方面是几何学上的相似性，例如：在左右图像中，同一外极线上出现的特征点顺序相同。特征特征是研究同一特征点后两帧中的位置变化，通过特征可以本文在特征前，先将图像等分为几个大小相同的区块，每一块的特征的，在特征过程中，对匹配好的特征点进行。本文是通过计算特征描述符间的欧氏距离完成特征的，但为了提高效率，特征点的查找范围缩小到当前特征点的某一固定大小的邻域内，如图5-6为特征示意图。图5-6特征示意右图像中上的特征点进行筛选，去掉其中没有上的特征点，保留匹配好同时可以成功的特征点，利用相机模型与相似三角形原理求解出两帧图像中的特征点在在机坐标系中的坐标。特征点三维坐标估计可以总结为以step1.k帧左图像中的特征点进行遍历，查找右图像中具有相同标号的step2.按照step1中遍历右图像中的特征点的方法，对第k+1帧图像进行特征点匹配，然后在第k+1帧左图像中找出与第k帧左图像中对应的特征点，如找不到则删除次对特征点，这一步也就是完成了特征点的。假设已知空间中一点在左右图像中的成像位置分别为(xl,xl)与(xr,xr)。相X (uu0)b/dY(vv)b/d Z

fb/ 通过特征匹配与特征可以得到特征点在相机坐标系下的坐标，PtX,YZt时刻的相机坐标系下的坐标，&Pt1XYZ 为该特征点在t+1时刻的机坐标系中的坐标。根据小孔模型和成像几何关u

x ux

x

s

0

y v=

f v0

R(r)

1

z11

xyzt时刻空间中某一特征点的坐标；s为相机光心与像平面0uv：空间点在图像参考系中的图像像素坐标；R(r：相机的旋转变化；T：相机的位置变化。XX偏Z滚俯Y5-75-7所示，偏航角、滚动角与俯仰角XYZRT

Tr= = 1

sin 5-3为相机的位姿变换矩阵，其中包含6个未知数，分别为三个方向step1.将5-4展开并对待求未知量tx,ty,tz与rx,ry,rz求偏导数step2.step1的结果，使用上一次计算结果作为txtytzrxryrz的初始值，第一次计算初始值全部设置为零，以表示待求变量估计值与真实值之间b11txb12tyb13tzb21txb22tyb23tz b2 b33tz

41txb42tyb43tzb4b51txb52tyb53tzb5b61txb62tyb63tz其 nfk0fk

nfk

fkbija

，Bi

(Ckfk0ak k （5-6）ki个变量求偏导数，其中ai、ajtxtytzrxryrz在某时刻的像素坐标表示为

Ckstep3.使用消元法求解5-6中的方程组，每一次都能求解出相应的i的step4.step3中得出的运动参数txtytzrxryrz，将当前时刻空间点三维时，使用当前的txtytzrxryrz对运动参数进行更新。使用-迭代求解出连续两帧之间的位姿变换矩阵，以初始帧相机坐通过计算出的当前运动参数，得到当前位姿信息。若当前帧是第一帧，初始化位姿变换矩阵为单位矩阵，并设当前位置为原点。对当前提取rf特征并描述，与词典中的单词对比，本文使用欧氏距离作为相似性度量的标准，当当前特征点的描述符与某单词之间的距离小于一定阈值时，则认为当前单词可以代表此特征点。对所有特征点进行遍历，使用直方图统计词典中每一单词oW1的向量，N为词典中单词个数。当当前位姿（t时刻）与之前某一帧（t-n时刻oWtnoW描述符之间的欧氏距离，若距oWt-noW描述符5-8为闭环检tn时刻图像间的位姿变化关系，最后进行位姿更新。5DetectZ0Z0Y

0X双目立体视觉定位中IMU信息的应2-6所示，{C}，{I}，{G}分别为相机坐标系，IMUIMU标定完成后，可以将视觉里程计定位结果本实验所采用的处理单元是便携式微型处理单元，如图5-9所示，其主要组成包括华硕微型主板，尺寸为17cmx17cm。CPU采用In酷睿i7处理器，483.4GHz4GDDR3SDRAM，其核1.5V，具有高带宽其低功耗的优点。此外，本处理单元还包括30G固态硬盘，主要用于安装操作系统与重要数据信息。整个系统使用锂图5-10视觉定位处理单本课题设计的实验在邮电大学校园内进行，将相机与IMU固连单元放时间，行人较多，实验绕邮电大学科研楼后小花园行走一圈，最后回到原点，行走距离大概二百米左右，全程耗时左右，最终回到原点后，系统可以检测到闭环。图5-11为实验过程中左相继所，左边第一张为 图5-11实验过程中的部分实验过程中采用两个线程，一个线程IMU数据，IMU可以实时输出三轴加速度，三轴角速度与融合后的旋转矩阵。另一个线程图像数据，实验过程中，在四个拐弯处静止几秒钟，输出拐弯处静止情况下的重力加速度，以重力加速度方向为法线方向，可以确定一个平面，根据四个点静止情况下的重力加速度，可以看出小车所走的平面是否水平。实验显示，科研楼后小花园基本水平，且双目的速度大概为80ms/。定位结果如图5-12所示。蓝色为里程计定位结果，绿色为将定位结果转换到大地坐标系下，在不考虑竖直方向的情况下，定位结果比较准确。根据实际情况竖直方向上应该没5-12X的增加竖直方向上也在增5-13可以看出将里程计定位结果转换到大地坐标系下，绿色线段所示，Z据的可用性且更接近真实值。5-12（左：x-y：x-zLinein3-D5DetectZ0Z

0

0

5-13x-y-本章小IMU总 123、实现了基于视觉词典（BoVW）4IMUIMU信息，为视觉提供大地平面，提高数据的可用性。展显然。本文实现了能够在现实环境中使用的立体视觉，但由于本人的水平和精力有限，本文所设计的立体视觉具有以下不足：2、闭环检测部分：在闭环检测部分词典生成的过程中，本文所使用的词的生成方法只使用了最简单的无监督聚类（-means）方法且无删除冗余单词，词典质量虽能满足使用要求但没有进行优化。可以通过概率统计的方法，对词典的质量进行优化，以提高 相似度检测的效率与准确度。[1].基于局部子图匹配的SLAM解决方法[D].:复旦大学[2].双目立休视觉与惯导融合里程估计方法[D].杭州:浙江大学[3]，彭高明，.PPS与测距法融合模型在机器人导航中的实现[J].机械工程与自2005,130（3：1~4[4],等.基于的移动机器人位子估计方法综综述[J].机器人,2002,24（2[5]，王光辉，等.自主移动机器人激光全局研究[J].机器人（5207[6]，峰.Line-feature-basedSLAMAlgorithm[J].自动化学报,2006,32（1）:43-KellyJ,SarialliS,SukhatmeGS.Combinedvisualandinertialnavigationforanunmannedaerialvehicle[C].FieldandServiceRobotics,2008:255-264.KonoligeK,AgrawalM，SolaJ:Large-scalevisualodometryforroughterrain,RoboticsResearch:Springer，2011:201-212.OskiperT,ZhuZ，SamarasekeraS，etal.Visualodometrysystemusingmultiplestereocamerasandinertialmeasurementunit[C].ComputerVisionandPatternRecognition,2007.CVPR'07.IEEEConferenceon,2007:1-8.TardifJ-P,GeorgeM，LaverneM，etal.Anewapproachtovision-aidedinertialnavigation[C].In ligentRobotsandSystems(IROS),2010IEEE/RSJInternationalConferenceon,2010:4161-4168.CorkeP,LoboJ,DiasJ.Anintroductiontoinertialandvisualsensing[J].TheInternationalJournalofRoboticsResearch,2007,26(6):519-535.MoravecHP.Obstacleavoidanceandnavigationintherealworldbyaseeingrobotrover[J].ObstacleAvoidance&NavigationintheRealWorldbyASeeingRobotRover,1980.RubleeE,RabaudV,KonoligeK,etal.ORB:anefficientalternativetoSIFTorSURF[C].IEEEInternationalConferenceonComputerVision,2011:2564-2571．MaimoneM,ChengY,MatthiesL.VisualOdometryontheMarsExplorationRoversIEEEInternationalConferenceonSystem,ManandCybernetics,2005.MaimoneM,ChengY,MatthiesL.TwoyearsofVisualOdometryontheMarsExplorationRovers:FieldReports[J].JournalofFieldRobotics,2007,24.MatthiesL,ShaferSA.Errormodelinginstereonavigation[J].Robotics&AutomationIEEEJournalof,1987,3(3):239-248.DavidN,OlegN,JamesB.Visualodometryforgroundvehicleapplications[J].JournalofFieldRobotics,2006,23(1):3-20.AgrawalM,KonoligeK.Real-timeLocalizationinOutdoorEnvironmentsusingStereoVisionandInexpensiveGPS[C]//PatternRecognition,2006.ICPR2006.18thInternationalC