基于视差空间运动估计的高精度立体视觉定位

第３９卷第１０期　．　光电工程　、，ｏ１．３９．ＮＯ．１０　２０１２年ｌ０月　Ｏｐｔｏ—Ｅｌｅｃｔｒｏｎｉｃ　Ｅｎｇｉｎｅｅｒｉｎｇ　０ｃｔ．２０１２　文章编号：１００３—５０１Ｘ（２０１２）１０—００９５—０８　基于视差空间运动估计的高精度立体视觉定位　许允喜　一，陈　（Ｉ．湖州师范学院信息与工程学院，浙江湖州３１３０００；　２．浙江大学信息与电子工程系，杭州３１００２７）　摘要：立体视觉定位算法的运动估计通常在３Ｄ欧式空间中进行，但由于特征点３Ｄ坐标的噪声各向异性且分布不　均匀，３Ｄ重建在深度方向上比另两个方向上的准确性差，从而导致３Ｄ欧式空间运动估计精确不高。本文提出了　一种新的基于视差空间运动估计的高精度立体视觉定位算法。算法首先采用视差空间４点闭环线性解法和　ＲＡＮＳＡＣ算法得到初始鲁棒运动估计和匹配内点。接着，利用新的视差空间再投影误差函数提出了基于ＬＭ算法　的视差空间运动参数非线性优化方法，对初始运动参数进一步优化。视差空间噪声分布均匀且各向同性，本文的　初始运动参数线性估计和非线性优化都在视差空间中进行且能达到全局最小。仿真实验和真实实验结果表明，本　文算法能得到高精度的立体视觉定位结果，优于传统的３Ｄ欧式空间运动估计方法。　关键词：立体视觉定位；视差空间；立体视觉；视觉导航　中图分类号：ＴＰ３９１．４１　文献标志码：Ａ　ｄｏｉ：１０．３９６９／ｊ．ｉｓｓｎ．１００３—５０１Ｘ．２０１２．１０．０１６　Ｓｔｅｒｅｏ　Ｖｉｓｕａｌ　Ｌｏｃａｌｉｚａｔｉｏｎ　Ｂａｓｅｄ　ｏｎ　Ｍｏｔｉｏｎ　Ｅｓｔｉｍａｔｉｏｎ　ｉｎ　Ｄｉｓｐａｒｉｔｙ　Ｓｐａｃｅ　ＸＵ　Ｙｕｎ．ｘｉ　，．．ＣＨＥＮ　Ｆａｎｇ　（１．Ｉｎｓｔｉｔｕｔｅ　ｏｆｉｎｆｏｒｍａｔｉｏｎ＆Ｅｎｇｉｎｅｅｒｉｎｇ，Ｈｕｚｈｏｕ　Ｔｅａｃｈｅｒｓ　Ｃｏｌｌｅｇｅ，Ｈｕｚｈｏｕ　３　１　３０００，Ｚｈｅｊｉａｎｇ　Ｐｒｏｖｉｎｃｅ，Ｃｈｉｎａ；　２．Ｄｅｐａｒｔｍｅｎｔ　ｏｆｉｎｆｏｒｍａｔｉｏｎ　Ｓｃｉｅｎｃｅ＆Ｅｌｅｃｔｒｏｎｉｃ　Ｅｎｇｉｎｅｅｒｉｎｇ，Ｚｈｅｊｉａｎｇ　Ｕｎｉｖｅｒｓｉｔｙ，Ｈａｎｇｚｈｏｕ　３　１００２７，Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：Ｔｈｅ　ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ　ｉｎ　ｓｔｅｒｅｏ　ｖｉｓｕａｌ　ｌｏｃａｌｉｚａｔｉｏｎ　ｉｓ　ｕｓｕａｌｌｙ　ｃａｒｒｉｅｄ　ｏｕｔ　ｉｎ　３Ｄ　Ｅｕｃｌｉｄｅａｎ　ｓｐａｃｅ．３Ｄ　ｐｏｉｎｔｓ　ａｒｅ　ｃｏｒｒｕｐｔｅｄ　ｗｉｔｈ　ｈｅｔｅｒｏｓｃｅｄａｓｔｉｃ　ａｎｄ　ａｎｉｓｏｔｒｏｐｉｃ　ｎｏｉｓｅ，ａｎｄ　ｔｈｅ　ｄｅｐｔｈ　ｄｉｒｅｃｔｉｏｎ　ｏｆ　３Ｄ　ｒｅｃｏｎｓｔｒｕｃｔｉｏｎ　ｉｓ　ｌｅｓｓ　ａｃｃｕｒａｔｅ　ｔｈａｎ　ｔｈｅ　ｔｗｏ　ｏｔｈｅｒ　ｄｉｒｅｃｔｉｏｎ．Ａｎｄ　ｔｈｅｒｅｆｏｒｅ，ｔｈｅ　ａｃｃｕｒａｃｙ　ｏｆ　ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ　ｉｎ　３Ｄ　Ｅｕｃｌｉｄｅａｎ　ｓｐａｃｅ　ｉｓ　ｌｏｗ．Ｗｅ　ｐｒｏｐｏｓｅ　ａ　ｎｅｗ　ｈｉｇｈ—ｐｒｅｃｉｓｉｏｎ　ｓｔｅｒｅｏ　ｖｉｓｕａｌ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｌｇｏｒｉｔｈｍ　ｂａｓｅｄ　ｏｎ　ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ　ｉｎ　ｄｉｓｐａｒｉｔｙ　ｓｐａｃｅ．Ｉｎｉｔｉａｌ　ｒｏｂｕｓｔ　ｍｏｔｉｏｎ　ｐａｒａｍｅｔｅｒｓ　ａｎｄ　ｍａｔｃｈｉｎｇ　ｉｎｌｉｅｒｓ　ａｒｅ　ｏｂｔａｉｎｅｄ　ｂｙ　ｆｏｕｒ　ｐｏｉｎｔｓ　ｃｌｏｓｅｄ—ｆｏｒｍ　ｌｉｎｅａｒ　ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ　ｉｎ　ｄｉｓｐａｒｉｙｔ　ｓｐａｃｅ　ｃｏｍｂｉｎｅｄ　ｗｉｔｈ　ＲＡＮＳＡＣ　ａｌｇｏｒｉｔｈｍ．Ｔｈｅｎ，ｗｅ　ｐｒｏｐｏｓｅ　ｎｏｎｌｉｎｅａｒ　ｍｏｔｉｏｎ　ｏｐｔｉｍｉｚａｔｉｏｎ　ｍｅｔｈｏｄ　ｂａｓｅｄ　ｏｎ　Ｌ—Ｍ　ａｌｇｏｒｉｔｈｍ　ｉｎ　ｔｈｅ　ｎｅｗ　ｄｉｓｐａｒｉｔｙ　ｓｐａｃｅ　ｒｅ－ｐｒｏｊｅｃｔｉｏｎ　ｅｒｒｏｒ　ｉｆｍｃｔｉｏｎ．Ｔｈｅ　ｍｏｔｈｏｄ　ｉｓ　ａｐｐｉｅｄ　ｔｏ　ｏｐｔｉｍｉｓｅ　ｔｈｅ　ｉｎｉｔｉａｌ　ｍｏｔｉｏｎ　ｐａｒａｍｅｔｅｒｓ．Ｔｈｅ　ｎｏｉｓｅ　ｉｎ　ｄｉｓｐａｒｉｔｙ　ｓｐａｃｅ　ｉｓ　ｈｏｍｏｇｅｎｅｏｕｓ　ａｎｄ　ｉｓｏｔｒｏｐｉｃ，ａｎｄ　ＯＵｒ　ｉｎｉｔｉａｌ　ｌｉｎｅａｒ　ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ　ａｎｄ　ｎｏｎｌｉｎｅａｒ　ｏｐｔｉｍｉｚａｔｉｏｎ　ｍｅｔｈｏｄ　ａｌｌ　ａｒｅ　ｃａｒｒｉｅｄ　ｏｕｔ　ｉｎ　ｄｉｓｐａｒｉｔｙ　ｓｐａｃｅ　ａｎｄ　ｃａｎ　ａｃｈｉｖｅ　ｇｌｏｂａｌｌｙ　ｍｉｎｉｍｕｍ．Ｓｉｍｕｌａｔｅｄ　ａｎｄ　ｒｅａｌ　ｅｘｐｅｒｉｍｅｎｔａｌ　ｒｅｓｕｌｔ　ｓｈｏｗ　ｔｈａｔ　Ｏ１２１＂ｍｅｔｈｏｄ　ｃａｎ　ａｃｈｉｖｅ　ｈｉｇｈｅｒ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｃｃｕｒａｃｙ　ｔｈａｎ　ｔｒａｄｉｔｉｏｎａｌ　ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ　ｍｅｔｈｏｄ　ｉｎ　３Ｄ　Ｅｕｃｌｉｄｅａｎ　ｓｐａｃｅ．　Ｋｅｙ　ｗｏｒｄｓ：ｓｔｅｒｅｏ　ｖｉｓｕａｌ　ｌｏｃａｌｉｚａｔｉｏｎ；ｄｉｓｐａｒｉｔｙ　ｓｐａｃｅ；ｓｔｅｒｅｏ　ｖｉｓｉｏｎ；ｖｉｓｕａｌ　ｎａｖｉｇａｔｉｏｎ　收稿日期　２０１２－０５－１４；收到修改稿日期：２０１２—０８—０６　基金项目　国家自然科学基金项目（６０８７２０５７）；浙江省自然科学基金项目（Ｒ１０９０２４４，Ｙ１１０１２３７，Ｙ１１１０９４４）；湖州市自然科学基金项目　（２Ｏｌ　１ＹＺ０７）；浙江省公益技术应用研究项目（２０１　１Ｃ２３　１３２）资助　作者简介　许允喜（１９７８．），男（汉族），江苏句容人。讲师，博士研究生，主要研究图像处理、计算机视觉以及人工智能等。　Ｅ—ｍａｉｌ：ｘｕｙｕｎｘｌ＠ｈｕｔｃ．ｚｊ．ｃｎ。　ｈｔｔｐ：／／ｗｗｗ　ｇｄｇｃ　ａｃ。ｃｎ　光电工程　２０１２牟１０月　０　引　言　智能车辆或智能移动机器人在自主导航过程中需要实时计算自身所处的位置，即实时定位问题　经典　的里程计从车轮的旋转估计机器人的运动，这在车轮打滑情况下会导致估计错误。ＧＰＳ信息不是在任何地　方都是有效的，例如森林、矿井、有电波干扰的战场、外星球等机器人应用场合。而利用立体相机进行运　动估计的立体视觉定位方法，也称视觉里程计，最近被成功应用Ｌ１　Ｊ，例如勇气号和机遇号火星车就采用了　这种立体视觉定位方法　。　立体视觉定位方法一般分为特征检测、特征匹配和运动估计三个计算步骤。目前，立体视觉定位的前　后帧运动估计一般都采用３Ｄ一３Ｄ运动估计方法　Ｊ，即３Ｄ欧式空间运动估计，但由于３Ｄ特征点在深度方　向上的不确定度分布不均匀，且噪声各向异性，无法得到最优估计，运动估计误差较大。视差空间的噪声　分布在理论上是各向同性的　…，文献［６—７】给出了一种直接在视差空间进行运动估计的计算方法，文献［８］　和［９］把该方法应用在立体视觉里程计算法中，获得了比３Ｄ欧式空间运动估计方法更高的计算精度。但该　方法只在旋转角度较小情况下适用，实验在近似直线运动情况下进行，不具有普遍性。文献［１０］提出了一　种基于新的视差空间误差投影函数的闭环线Ｊ陛运动估计解法，适用于任意运动情况。　本文提出了一种新的基于视差空间运动估计的高精度立体视觉定位算法。算法首先采用文献【１Ｏ］提出　的视差空间闭环线性解法和ＲＡＮＳＡＣ算法得到初始运动参数和匹配内点。接着，基于新的视差空间误差　投影函数提出了基于Ｌｅｖｅｎｂｅｒｇ－Ｍａｒｑｕａｒｄ算法的视差空间运动参数优化方法进一步优化初始运动参数。　本文不详细讨论立体视觉定位的特征检测和特征匹配：特征检测采用ＣｅｎＳｕｒＥ特征检测子　“，特征匹　配采用Ｕ—ＳＵＲＦ描述符　匹配。本文安排如下：第１节介绍了视差空间闭环线性估计算法；第２节描述了　基于视差空间线性闭环算法的初始鲁棒运动估计；第３节提出了基于Ｌｅｖｅｎｂｅｒｇ－Ｍａｒｑｕａｒｄ算法的视差空间　运动优化方法；第４节对本文算法进行了仿真和真实智能车实验。实验结果表明：本文方法是精确的，鲁　棒的，优于传统的３Ｄ欧式空间运动估计方法。最后一节对本文进行了总结。　１　视差空间闭环线性运动估计算法　ｈｔｔｐ：／／ｗｗｗ．ｇｄｇｃ　ａｃ．ｃｒｌ　茎　查簦　塑　垒塞！　三基于视差空间运动估计的高精度立体视觉定位　（３）　令向量∞＝Ｅ］表示视差空间，令　＝［　，　一　表示视差空间特征点的齐次坐标。　１．２视差空间线性闭环运动算法　１●－＝●　＿＿　●ｊ　三＿　二二Ｊ　Ｏ　～厂　设摄像机运动前后两帧中对应特征匹配点的　３Ｄ坐标分别为Ｐ＝（　，】，，ｚ）　和Ｐ　＝（　，ｙ　，Ｚ　）　，假定Ｒ　一　０　一厂　和ｔ分别为旋转和平移的变换参数，则有：　Ｚ　１　Ｏ＝　．　厂Ｏ　０　０　０　０　１　由式（３）和（４）可得出下列方程：　一，　．　．　—．．．．．．　．．．．．Ｌ　］　０ｆ厂０　０　０　０　０　１　（４）　ｙ　＝　］●●●●ｊ鱼＿　Ｏ　０　　．０　０　Ｏ　．Ｏ　　—．．．．．．．．．．：　．．．．．Ｌ　１●　●●●●　●●●　●Ｊ　１　●●●●　●　●●●Ｊ—．．．．．．．．．．．．．．．Ｌ　Ｂ　—．．　．．．．．．，．．Ｌ　，０　Ｏ　Ｏ　０　Ｏ　０　尼Ｏ　詹ｆ‘０　ｌＲ　０　　１　（５）　　—　一］　●　●－＿１　风　０　０　０　ｌ　．．．．．．．．。．．．．．。．．．．．．．．．．．．．．．．．．Ｌ—．厂．．。．．．．．．．．Ｏ　０　０　．．．．．．．．．．　．．．．．．．．．．．Ｌ　—厂Ｏ　Ｏ　０　０　０　，　．　０　儿ＩＩ　０Ｉ１●●　●●●●●●●●ｊ　＿．．．．　．．．．ｙ　Ｚ．．．．．．．．．　．．．．。．．．●　．Ｌ　厂０　Ｏ　０／０　０　设　Ｈ（Ｒ，ｆ）　０　０　０　１　Ｏ　Ｏ　０　１　　一（６）　Ｏ　０　．　０　Ｏ　Ｏ　．　０　　１　●●　●●●●●　●ｊ由式（５）和（６）得到下列视差空间投影函数［１０］：　—．．．．．．．．．．。．．．．．．．．．．．．．．．．。．．．．．Ｌ１　　一　１●●●●　●　●●●●ｊ　一　“’＝　，　ｗ　葡’　酵　（７）　鲁　”，　由式（６）得到Ｈ（Ｒ，ｔ）：　Ｂ　－厂　１　ｔ　０　卉设估计的单应矩阵如下式：　ｒ３３　Ｈ＝　（８）　式（７）可表达为如下的线性方程㈣：　ＺｉＯ＝Ｃｆ　０　０一　ｆ—ＵｔＶｉ—Ｕ＇ｄｉ　（９）　式中：　ｄｌ　１一ｖ　－ｖ：ｖ￣一ｖ　ｄｉ　０　０一ｄ：ｕ　一ｄ：ｖｉ—ｄｉ＇ｄｉ　ｈｔｔｐ：／／ｗｗｗ。ｇｄｇｃ。ａｃ。Ｃｌ－ｉ　９８　光电工程　２０１２年１０月　０＝　ｈ　ｈ３　ｈ４　ｈ　ｈ　ｈ７　ｈ８　ｈ９　ｈ。。ｈ　ｌ　ｈ。：］，　每个匹配特征点有３个约束方程，共有１２个待估计参数，所以求解这些参数需要４对匹配特征点：　０＝Ｚ　ｃ　（１１）　式中：ｚ：［　ｚ　…ｚ　】　，ｃ：［ｃ　ｃ　Ｔ　…ｃ　】　。　０参数确定后旋转矩阵Ｒ和平移ｔ可由下式计算得到㈣：　ｌ　ｈ１　ｈ２　ｈ４／ｆ　ｌ　Ｒ：Ｉ　ｈ　ｈ６　ｈ　／ｆ　ｌ　ｌ　ｆ・ｈ９　ｆ・ｈ　。ｈ　２　Ｉ　Ｂ・ｈ３　＾　（１２）　Ｂ・ｈ７　＾　（１３）　厂・Ｂ・ｈ１ｌ　２基于视差空间线性算法的初始鲁棒运动估计　在特征检测和匹配阶段，由于特征检测和匹配难免会出现错误匹配和图像位置偏差，直接利用摄像机　运动前后帧的特征匹配进行运动估计是不鲁棒的。本文采用ＲＡＮＳＡＣ鲁棒参数估计方法　引，结合第１节　的视差空间线性闭环运动估计算法进行摄像机初始鲁棒运动估计，步骤如下：　１）在匹配特征对选择４对，利用第１节的视差空间线性闭环运动估计算法计算旋转矩阵和平移参数。　２）计算满足该运动模型的匹配点对数。各匹配对相对于该运动模型的误差计算如下式，即视差空间投　影误差：　—．。．．．．，．．．．．．．＝　，．．．．．　．．Ｌ　一　０　Ｏ　口Ｄ￣（Ｕｔ－－　）＜ｆ，　ｂｓ（ｖ＇－　∞　）＜ｚ’，口ｂｓ（ｄ，一　∞　）＜ｚ－　∞　（１４）　误差小于某固定阂值，则该特征匹配对为匹配内点。　　１●　●，，●　，ｊ３）重复步骤１）和２）若干次。　　’４）选取内点数最多的估计参数和点集作为初始运动估计参数和匹配内点。　３视差空间运动参数优化　，Ｌ１　　由上述闭环线性运动估计算法求解的旋转矩阵Ｒ不是正交矩阵。在利用Ｌｅｖｅｎｂｅｒｇ—Ｍａｒｑｕａｒｄ算法进行　运动参数优化前需要把　矩阵转化为标准的正交矩阵。把估计的矩阵转化为Ｆｒ０　ｏｂｅｎｉｕｓ范数下最逼近的正　、　交矩阵可以通过ＳＶＤ算法解决：　：ＵＷＶＴ≈ＵＩＶＴ≈ＵＶＴ　接着，对旋转　用四元数表达：　ｌ　ｇ　＋ｑ　一ｇ　—ｇ；　２（ｇ１ｑ２一ｇｏｇ３）　２（ｇｌｑ３＋ｑｏｑ２）Ｉ　Ｒ＝Ｉ　２（ｇ】ｑ２＋ｑｏｑ３）　～ｑ　ｑ；一ｑ；　２（ｑ２ｑ３一ｑｏｑ１）Ｉ　（１５）　ｉ　２（ｇ　ｑ３一ｑｏｑ２）　２（ｑ２鸟３＋ｇ０ｇ　）　ｇ　一ｇ　一ｇ；＋ｇ　Ｉ　其中：ｑ　＋ｑ　＋ｑ；＋ｑ　：１。ｘ＝（ｑ　ｑ　ｑ，ｔ　ｔｙ　ｔｚ）　为表示旋转和平移的６个未知参数。　设共有，２个匹配内点，则视差空间投影误差平方和为　Ｆ（ｘ）＝Ｚ（ｆｊ（ｘ）一哆，）　（　（ｘ）一面，）　ｊ＝ｌ　（１６）　ｆ（ｘ）为视差空间投影函数，即式（７）。面＝（　）　为特征点视差空间中的测量位置。该问题可以通　过Ｌｅｖｅｎｂｅｒｇ－Ｍａｒｑｕａｒｄ非线性最小平方迭代算法求解，其在视差空间求出使投影误差最小的运动参数Ｘ。　ｈｔｔｐ：／／ｗｗｗ。ｇｄｇｃ　ａｃ。ｃｎ　第３９卷第１０期　许允喜，等：基于视差空间运动估计的高精度立体视觉定位　在计算参数更新步长时需求解下列方程：　（ｔ，Ｔ　一。Ｊ＋ＸＩ）ｒ￣＝ＪＴ　（　（　）一　）　（　）　ｏ　．，：　一　（　）　Ｏｘ１　（１７）　≥０为控制参数，使得．，　Ｊ＋　是正定矩阵。在每次迭代后，当投影误差减少时，则　减少一个　因子，而当投影误差增加时，则　增加一个因子。视差空间中（“ｖ　ｄ）的噪声分布是确定的。“、ｖ图像坐　标的噪声是由于图像离散化和图像特征提取算法引起的。所有特征点的噪声方差　和　都一样，即　＝　：　。视差ｄ的噪声为立体匹配的不确定引起。噪声方差　由采用的立体匹配方法决定，一般情况　下　＝　。由以上分析可知，视差空间中（Ｕ　ｄ）噪声方差是独立的且分布均匀。所以任何（Ｕ　ｄ　）协　三　＝［喜　三　量］　一　饥一　一　４）计算８ｘ＝（Ｊ　Ｊ＋　，）　Ｊ　（／（　）一面），贝０　ｎｅ、Ⅳ＝　＋　。　６）若　｝＜　，停止计算，输出运动参数。否则置　：　，　：ｘ　，转４）。　７）若　ｊ＜　，停止计算，输出运动参数。否则置　：Ｘ／６，＇，　：　，转４）。　、／７、　分　４实验　为了验证本文算法的有效性，本文进行了仿真实验和真实智能车实验，并与３Ｄ欧式空间运动估计算　法［４－５］和广义正交迭代运动估计算法【Ｊ　ｂＪ进行了对比。　４．１仿真实验　为了验证本文算法的优越性，建立了立体视觉定位算法仿真实验。实验条件为：立体相机的等效焦距　为４８０，基线长度为０．２４　ｍ，图像大小为６４０ｘ４８０。在立体匹配和前后帧匹配中都加入高斯噪声和３０％的　误匹配。实验共产生４００帧序列，每帧进行随机运动。ＥＳ　Ｅ表示基于３Ｄ欧式空间运动估计的立体视觉　定位方法，ＯＳ　ＭＥ表示基于广义正交迭代运动估计算法的立体视觉定位方法，ＤＳ＿ＭＥ表示本文的基于视　差空间运动估计的立体视觉定位方法。如图１给出了一组真实行进轨迹和二种算法运动估计轨迹的对比图，　相对于Ｅｓ—ＭＥ和Ｏｓ．ＭＥ方法，ＤＳ—ＭＥ方法的运动估计估计更接近于基准运动路线。行进终点处３Ｄ位　置的均方根误差定义为√（　）　＋（ＡＹ）　＋（Ａｚ）。　姿态角度的均方根误差定义为√（△　＋（Ａｏ）　＋（△　）　。　表１给出了不同噪声水平下Ｅｓ—ＭＥ、ｏｓ—ＭＥ和ＤＳ—ＭＥ方法的立体视觉定位误差。很显然，ＤＳ＿ＭＥ方法　定位误差更小，定位结果更精确。　ｈｔｔｐ：ｌｌｗｗｗ。ｇｄｇｃ　ａｃ　ｃｎ　１Ｏ０　光电工程　２０１２年１０月　８　４　Ｏ　４　０　２０　４０　６Ｏ　８０　１００　１２０　ｚ／ｍ　图１仿真实验运动轨迹对比图　图２智能车　Ｆｉｇ．１　Ｔｈｅ　ｃｏｍｐａｒｉｓｏｎ　ｏｆｍｏｔｉｏｎ　ｔｒａｊｅｃｔｏｒｉｅｓ　ｏｆｓｉｍｕｌａｔｅｄ　ｅｘｐｅｒｉｍｅｎｔ　Ｆｉｇ．２　Ｔｈｅ　ｉｎｔｅｌｌｉｇｅｎｔ　ｖｅｈｉｃｌｅ　表１　三种运动估计方法的立体视觉定位误差　Ｔａｂｌｅ　１　Ｓｔｅｒｅｏ　ｖｉｓｕａｌ　ｌＯＣａｌｉｚａｔｉｏｎ　ｅｒｒｏｒ　ｆｏｒ　ｔｈｒｅｅ　ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ　ｍｅｔｈｏｄ　茸｝ｚ　加　ｍ　ｏ　４．２真实智能车实验　４．２．１实验１　本文采用‘＇Ｎｅｗ　Ｃｏｌｌｅｇｅ　Ｄａｔａｓｅｔ”（感谢牛津大学移动机器人研究小组提供）进行了智能车立体视觉定位　实验。智能小车装有窄基线立体相机，基线长度约为０．１２　ｍ，如图２所示。　实验采用数据集中的一个闭环路线，如图３所示。闭环路线为绕１个“圆”形行进２周。图４给出了　ＥＳＭＥ、ＯＳ＿—ＭＥ和ＤＳ＿ＭＥ方法的运动估计轨迹。由于行进路线为闭环路线，所以终点处位置和方向误差　图３　牛津大学数据集的航拍位置　图４定位算法的运动估计轨迹对比　Ｆｉｇ・３　Ｏｘｆ０ｒｄ　ｄａｔａ：ａｅｒｉａｌ　ｖｉｅｗ　ｏｆｌｏｃａｔｉｏｎ　Ｆｉｇ．４　Ｅｓｔｉｍａｔｅｄ　ｍｏｔｉｏｎ　ｒｔａｊｅｃｔｏｒｉｅｓ　—表２闭环误差统计　Ｔａｂｌｅ　２　Ｌｏｏｐ　ｃｌｏｓｕｒｅ　ｅｒｒｏｒ　ｓｔａｔｉｓｔｉｃｓ　ｈｔｔｐ：ｌｌｗｗｗ．ｇｄｇｃ　ａｃ　ＣＮ　．　珈　第３９卷第１０期　许允喜，等：基于视差空间运动估计的高精度立体视觉定位　！　！　即为闭环误差，如表２所示。从表２的结果可以看出：相对于ＥＳ—ＭＥ和ＯＳ—ＭＥ方法，　ＤＳ＿ＭＥ方法闭环　位置和姿态误差都更小，定位结果更精确。　４。２．２实验２　实验２采用法国尼斯大学Ａｎｄｒｅｗ　Ｃｏｍｐｏｒｔ数据集。智能汽车装有宽基线立体相机，在城市道路上行　驶，立体相机基线长度约为１　ｍ。该数据集较实验１的数据集更具挑战性，立体匹配误差很大。实验２采　用数据集中的一个闭环路线。图５给出了该数据集一组立体采样图像和航拍位置。图６给出了ＥＳＭＥ、　＿ＯＳ—ＭＥ和ＤＳ—ＭＥ方法的运动估计轨迹。闭环误差如表３所示。从表３的结果可以看出：相对于ＥＳＭＥ　—和Ｏｓ＿ＭＥ方法，Ｄｓ．ＭＥ方法闭环位置和姿态误差都更小，定位结果更精确。　Ｏ　一２０　．．４０　２０　０　２０　ｚ／ｍ　图５尼斯大学数据集的航拍位置　图６定位算法的运动估计轨迹对比　Ｆｉｇ．５　Ｎｉｃｅ　ｄａｔａ：ａｅｒｉａｌ　ｖｉｅｗ　ｏｆｌｏｃａｔｉｏｎ　Ｆｉｇ．６　Ｅｓｔｉｍａｔｅｄ　ｍｏｔｉｏｎ　ｔｒａｊｅｃｔｏｒｉｅｓ　表３闭环误差统计　Ｔａｂｌｅ　３　Ｌｏｏｐ　ｃｌｏｓｕｒｅ　ｅｒｒｏｒ　ｓｔａｔｉｓｔｉｃｓ　５结论　本文提出了一种新的立体视觉导航定位算法，算法利用了视差空间噪声分布各向同性的性质，在视差　空间内优化运动估计参数。实验结果表明，算法具有高精度、鲁棒性强的优点，优于传统的基于３Ｄ欧式　空间的运动估计算法和和广义正交迭代运动估计算法。　参考文献：　［１１］　Ｋｏｎｏｌｉｇｅ　Ｋ，Ａｇｒａｗａｌ　Ｍ，Ｂｌａｓ　Ｍ，ｅｔ　ａ１．Ｍａｐｐｉｎｇ，ｎａｖｉｇａｔｉｏｎ，ａｎｄ　ｌｅａｒｎｉｎｇ　ｆｏｒ　ｏｆｆ－ｒｏａｄ　ｔｒａｖｅｒｓａｌ［Ｊ］．Ｊｏｕｒｎａｌ　ｏｆ　Ｆｉｅｌｄ　Ｒｏｂｏｔｉｃｓ（Ｓ１５５６—４９５９），２００９，２６（１）：８８一ｌ１３．　［２］Ｋ￣ｅｉｐ　Ｌ，Ｃｈｌｉ　Ｍ，Ｓｉｅｇｗａｒｔ　Ｒ。Ｒｏｂｕｓｔ　Ｒｅａｌ—Ｔｉｍｅ　Ｖｉｓｕａｌ　Ｏｄｏｍｅｔｒｙ　ｗｉｔｈ　ａ　Ｓｉｎｇｌｅ　Ｃａｍｅｒａ　ａｎｄ　ａｌｌ　ＩＭＵ［ｃ］／／Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　Ｂｒｉｉｔｓｈ　Ｍａｃｈｉｎｅ　Ｖｉｓｉｏｎ　Ｃｏｎｆｅｒｅｎｃｅ（ＢＭＶＣ），Ｄｕｎｄｅｅ，ＵＫ，Ａｕｇ　２９一Ｓｅｐ　１，２０１　１：１—１　１．　【３］　Ｃｏｍｐｏｒｔ　Ａ　Ｉ，Ｍａｌｉｓ　Ｅ，Ｒｉｖｅｓ　Ｐ．Ｒｅａｌ—ｔｉｍｅ　Ｑｕａｄｒｉｆｏｃａｌ　Ｖｉｓｕａｌ　Ｏｄｏｍｅｔｒｙ［Ｊ］．Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｊｏｕｒｎａｌ　ｏｆ　Ｒｏｂｏｔｉｃｓ　Ｒｅｓｅａｒｃｈ（Ｓ０２７８－３６４９），２０１０，２９（２／３）：２４５—２６６．　［４］ＭＡＩＭＯＮＥ　Ｍ，ＣＨＥＮＧ　Ｙ，ＭＡＴＴＨＥＩＳ　Ｌ．Ｔｗｏ　ｙｅａｒｓ　ｏｆｖｉｓｕａｌ　ｏｄｏｍｅｔｒｙ　ｏｎ　ｔｈｅ　ｍａｒｓ　ｅｘｐｌｏｒａｔｉｏｎ　ｒｏｖｅｒｓ［Ｊ］．Ｊｏｕｒｎａｌ　ｏｆ　Ｆｉｅｌｄ　Ｒｏｂｏｔｉｃｓ（Ｓ１５５６—４９５９），２００７，２４（３）：１６９—１８６．　［５］彭勃，周文晖，刘济林．基于Ｈａｒｒｉｓ角点检测的立体视觉里程计［Ｊ］．兵工学报，２００７，２８（１２）：１４９８—１５０２。　ＰＥＮＧ　Ｂｏ，ＺＨＯＵ　Ｗｅｎ—ｈｕｉ，ＬＩＵ　Ｊｉ—ｌｉｎ．Ｈａｒｒｉｓ　Ｃｏｍｅｒ　Ｄｅｃｔｅｃｔｉｏｎ—ｂａｓｅｄ　Ｓｔｅｒｅｏ　Ｖｉｓｕａｌ　ｏｄｏｍｅｔｒｙ［Ｊ］．Ａｃｔａ　Ａｒｍａｍｅｎｔａｉｉ，２００７，　２８（１２）：１４９８—１５０２．　．　ｈｔｔｐ：ｌｌｗｗｗ，ｇｄｇｃ　ａｃ　ｃｎ　１０２　光电工程　２０１２年１０月　［６　Ｄｅｍｉ６］ｒｄｊｉａｎ　Ｄ，Ｄａｒｒｅｌｌ　Ｔ．Ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ　ｆｒｏｍ　ｄｉｓｐａｒｉｔｙ　ｉｍａｇｅ［Ｃ］／／Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ｉｎｔｅｒｎａｔｉｏｎａｌ　ｖｏｎｆｅｒｅｎｅｅ　ｏｎ　ｃｏｍｐｕｔｅｒ　ｖｉｓｉｏｎ，Ｖａｎｃｏｕｖｅｒ，ＢＣ，Ｕｎｉｔｅｄ　ｓｔａｔｅｓ，Ｊｕｌｙ　９－１２，２００１：６２８—６３５．　［７］Ｄｅｍｉｒｄｊｉａｎ　Ｄ，Ｄａｒｒｅｌｌ　Ｔ．Ｕｓｉｎｇ　ｍｕｌｔｉｐｌｅ－ｈｙｐｏｔｈｅｓｉｓ　ｄｉｓｐａｒｉｙ　ｍａｔｐｓ　ａｎｄ　ｉｍａｇｅ　ｖｅｌｏｃｉｙ　ｆｔｏｒ　３－Ｄ　ｍｏｔｉｏｎ　ｅｓｔｉｍａｔｉｏｎ［Ｊ】　＿Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｊｏｕｒｎａｌ　ｏｆＣｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ（Ｓ０９２０—５６９１），２００２，４７（１／３）：２１９～２２８．　［８】　吴功伟，周文晖，顾伟康．基于视差空间的双目视觉里程计［Ｊ］．传感技术学报，２００７，２０（６）：１４３２—１４３６．　ＷＵ　Ｇｏｎｇ—ｗｅｉ，ＺＨＯＵ　Ｗｅｎ—ｈｕｉ，ＧＵ　Ｗｅｉ－ｋａｎｇ．Ｄｉｓｐａｒｉｔｙ　Ｓｐａｃｅ　Ｂａｓｅｄ　Ｂｉｎｏｃｕｌａｒ　Ｖｉｓｕａｌ　Ｏｄｏｍｅｔｒｙ［Ｊ］．Ｃｈｉｎｅｓｅ　Ｊｏｕｒｎａｌ　ｏｆ　Ｓｅｎｓｏｒｓ　ａｎｄ　Ａｃｔｕａｔｏｒｓ，２００７，２Ｏ（６）：１４３２～１４３６．　［９】李智，周丈晖，刘济林．动态场景下基于视差空间的立体视觉里程计［Ｊ］．浙江大学学报：工学版，２００８，４２（１０）：１６６１—１６６５．　ＬＩ　Ｚｈｉ，ＺＨＯＵ　Ｗｅｎ－ｈｕｉ，ＬＩＵ　Ｊｉ—ｌｉｎ．Ｓｔｅｒｅｏ　ｖｉｓｕａｌ　ｏｄｏｍｅｔｒｙ　ｆｒｏｍ　ｄｉｓｐａｒｉｙｔ　ｓｐａｃｅ　ｉｎ　ｄｙｎａｍｉｃ　ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｊ］．Ｊｏｕｒｎａｌ　ｏｆ　Ｚｈｅｊｉａｎｇ　Ｕｎｉｖｅｒｓｉｔｙ：Ｅｎｇｉｎｅｅｒｉｎｇ　Ｓｃｉｅｎｃｅ，２００８，４２（１０）：１６６１—１６６５．　［１０】Ｄｅｒｐａｎｉｓ　Ｋ　Ｇ，Ｃｈａｎｇ　Ｐ．Ｃｌｏｓｅｄ－ｆｏｒｍ　Ｌｉｎｅａｒ　Ｓｏｌｕｔｉｏｎ　Ｔｏ　Ｍｏｔｉｏｎ　Ｅｓｔｉｍａｔｉｏｎ　Ｉｎ　Ｄｉｓｐａｒｉｔｙ　Ｓｐａｃｅ［Ｃ】／／Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ＩＥＥＥ　Ｉｎｔｅｌｌｉｇｅｎｔ　Ｖｅｈｉｃｌｅ　Ｓｙｍｐｏｓｉｕｍ，Ｔｏｋｙｏ，Ｊａｐａｎ，Ｊｕｎｅ　１３—１５，２００６：２６８—２７５．　［１　１】ＡＧＲＡＷＡＬ　Ｍ，ＫＯＮＯＬＩＧＥ　Ｋ．ＣｅｎＳｕｒＥ：Ｃｅｎｔｅｒ　ｓｕｒｒｏｕｎｄ　ｅｘｔｒｅｍａｓ　ｆｏｒ　ｒｅａｌｔｉｍｅ　ｆｅａｔｕｒｅ　ｄｅｔｅｃｔｉｏｎ　ａｎｄ　ｍａｔｃｈｉｎｇ［Ｅｌ／／　Ｐｒｏｃｅｅｄｉｎｇ　ｏｆ　１０ｔｈ　Ｅｕｒｏｐｅａｎ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｃｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ，Ｍａｒｓｅｉｌｌｅ，Ｆｒａｎｃｅ，Ｏｃｔｏｂｅｒ　１２－１８，２００８：１０２—１　１５，　［１２］Ｂａｙ　Ｈ，Ｔｕｙｔｅｌａａｒｓ　Ｔ，Ｖａｎ　Ｃｏｏｌ　Ｌ．ＳＵＲＦ：Ｓｐｅｅｄｅｄ　ｕｐ　ｒｏｂｕｓｔ　ｆｅａｔｕｒｅｓ［Ｊ］．Ｃｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ　ａｎｄ　Ｉｍａｇｅ　ＵⅡｄｅｒｓｔａｎｄｉｎｇ（Ｓ　１０７７—３１４２），２００８，１１０（３）：３４６—３５９．　［１３］Ｍａｒｔｉｎ　Ａ　Ｆ，Ｒｏｂｅｔｒ　Ｃ　Ｂ．Ｒａｎｄｏｍ　ｓａｍｐｌｅ　ｃｏｎｓｅｎｓｕｓ：ｆｌ　ｐａｒａｄｉｇｍ　ｏｆｒ　ｍｏｄｅｌ　ｉｆ￣ｉｎｇ　ｗｉｔｈ　ａｐｐｌｉｃａｔｉｏｎｓ　ｔｏ　ｉｍａｇｅ　ａｎａｌｙｓｉｓ　ａｎｄ　ａｕｔｏｍａｔｅｄ　ｃａｒｔｏｇｒａｐｈｙ［Ｊ］．Ｃｏｍｍｕｎ．ＡＣＭ（Ｓ０００１—０７８２），１９８１，２４（６）：３８１—３９５．　［１４］王江涛，陈得宝，杨新军，等．一种高效的实时视觉姿态测量［Ｊ］．光电工程，２０１１，３８（１）：７６—８０．　ＷＡＮＧ　Ｊｉａｎｇ　ｔａ０，ＣＨＥＮ　Ｄｅ－ｂａｏ，ＹＡＮＧ　Ｘｉｎ－ｊｕｎ，ｅｔ　ａ１．Ｅｆｉｆｃｉｅｎｔ　ａｎｄ　Ｒｅａｌ—ｔｉｍｅ　Ｖｉｓｉｏｎ—ｂａｓｅｄ　Ｐｏｓｅ　Ｅｓｔｉｍａｔｉｏｎ［Ｊ］．Ｏｐｔｏ—　Ｅｌｅｃｔｒｏｎｉｃ　Ｅｎｇｉｎｅｅｒｉｎｇ，２０１　１，３８（１）：７６—８０．　［１５］许允喜，蒋云良，陈方．多摄像机系统位姿估计的广义正交迭代算法［Ｊ］．光学学报，２００９，２９（１）：７２—７７．　ＸＵ　Ｙｕｎ—ｘｉ，ＪＩＡＮＧ　Ｙｕｎ—ｌｉａｎｇ，ＣＨＥＮ　Ｆａｎｇ．Ａ　ｇｅｎｅｒａｌｉｚｅｄ　ｏｒｔｈｏｇｏｎａｌ　ｉｔｅｒａｔｉｖｅ　ａｌｇｏｒｉｔｈｍ　ｆｏｒ　ｐｏｓｅ　ｅｓｔｉｍａｔｉｏｎ　ｏｆ　ｍｕｌｔｉｐｌｅ　ｃａｍｅｒａ　ｓｙｓｔｅｍｓ［Ｊ］．Ａｃｔａ　Ｏｐｔｉｅａ　Ｓｉｎｉｅａ，２００９，２９（１）：７２—７７．　［１６】许允喜，蒋云良，陈方．基于广义正交迭代算法的立体视觉定位［Ｊ］．光子学报，２０１１，４０（８）：１２２５—１２３０．　ＸＵ　Ｙｕｎ。ｘｉ，ＪＩＡＮＧ　Ｙｕｎ‘ｌｉｎａｇ，ＣＨＥＮ　Ｆａｎｇ．Ｓｔｅｒｅｏ　ｖｉｓｕａｌ　ｌｏｃａｌｉｚａｔｉｏｎ　ｂａｓｅｄ　ｏｎ　ｇｅｎｅｒａｌｉｚｅｄ　ｏｒｔｈｏｇｏｎａｌ　ｉｔｅｒａｔｉｖｅ　ａｌｇｏｒｉｔｈｍ［Ｊ］．　Ａｃｔａ　Ｐｈｏｔｏｎｉｃａ　Ｓｉｎｉｃａ，２０１　ｌ，４Ｏｆ８）：１２２５－１２３０．　ｈｔｔｐ：１１ＷＷＷ　ｇｄｇｃ　ａｃ　ｃＲ