文章快速检索  
  高级检索
一种地标点修正的高精度双目视觉导航方法
张洋, 王新龙    
北京航空航天大学 宇航学院, 北京 100191
摘要:自主导航技术是无人机、智能机器人以及智能车辆等技术领域中的一项关键技术。为了克服传统视觉导航方法存在的不足,以摄像机自运动估计为理论基础,提出了一种使用地标点修正的高精度双目视觉导航方法。该方法从连续图像序列的帧间变化中提取摄像机自运动信息,再通过速度、角速度积分求得载体的位置姿态等参数。同时,通过引入地标点提供的绝对定位信息,进一步提高了长时间导航的精度。将地标点绝对定位与双目视觉相对定位相结合,并对双目视觉量测噪声建模和解相关,抑制了单纯使用双目视觉长时间导航时误差的积累发散。仿真结果表明此方法具有精度高、自主性强、导航信息完备等优点。
关键词双目视觉     视觉导航     地标点     高精度导航     自主导航    
Landmark fixed high-precision binocular visual navigation method
Zhang Yang, Wang Xinlong     
School of Astronautics, Beijing University of Aeronautics and Astronautics, Beijing 100191, China
Abstract:Autonomous navigation is a crucial technology in the field of unmanned aerial vehicle (UAV), intelligent robots and smartcars. To overcome shortages in traditional visual navigation methods, a landmark fixed high-precision binocular visual autonomous navigation method, based on the camera egomotion estimation theory, was put forward. In this method, camera's egomotion parameters, velocity and angular velocity, were measured from the variance between frames of the continuing image sequence, then the position and attitude was calculated by accumulating the velocity and angular velocity. Moreover, by bringing in the absolute positioning information provided by the landmarks, the performance of the method in long time navigation was improved. Combining the absolute positioning information from the landmarks and the relative navigation information from the binocular vision, as well as modeling and decoupling the measurement noise of the binocular vision, the accumulation and increasing of the navigation error was suppressed. Simulation result shows that this method has the advantage of high precision, autonomy and completeness of navigation information.
Key words: binocular vision     visual navigation     landmark     high-precision navigation     autonomous navigation    

视觉导航与卫星导航、惯性导航、地磁导航等相比具有结构简单、适应性强、相互干扰小、成本低和环境信息丰富等优点.视觉导航技术在许多研究领域得到了重视和发展,其应用范围涵盖了海、陆、空、天等环境,如海洋环境下基于视觉海底拼图和水下标记的视觉导航[1, 2];空天环境下的无人机视觉导航[3]、月面着陆的视觉导航[4]等应用;陆地环境下的智能移动车辆[5]、移动机器人系统[6]等都大量使用了视觉导航技术,它已经成为提高机械智能化水平必不可少的系统装备技术,因此视觉导航技术的研究具有重要意义.

传统视觉导航根据地图的使用情况,可分为3类[7]:基于地图的导航方法、地图建立型导航方法以及无地图导航方法.基于地图的导航方法通过将视觉传感器获得的图像与预先存储的地图进行匹配,获取当前载体的位姿信息,这种方法精度高但运算量大、实时性差[8].地图建立型导航方法可以在线建立载体附近小范围内的地图,用于导航与定位,这种方法对陌生环境具有良好的适应性,但是导航过程缓慢,仅适用于低速机器人小范围内的导航等[9].无地图导航方法不需要对环境信息进行全面描述.光流法、基于特征跟踪或外观的导航方法是无地图视觉导航方法的主要研究方向.这类导航方法运算量较小、实时性好,但由于缺少绝对参考,导航误差会积累发散[10].

为了解决无地图导航方法中导航误差会积累发散的问题,本文结合摄像机自运动参数估计理论,提出了一种利用地标点修正的高精度双目视觉导航方法.通过对地貌特征点进行跟踪,估计载体的运动参数,同时为了限制误差的积累发散,引入地标点信息对导航误差予以修正.该方法不需要人为设置导航特征,运动参数的提取完全来自地貌特征点,具有较强的自主性.同时只需预先在电子地图上标定一些特征点用于导航误差校正,避免了复杂的地图匹配运算,精度高且运算量小、实时性好.

1 基于双目视觉的摄像机自运动估计原理

从视频图像序列中获取摄像机运动信息的过程即为摄像机自运动估计(camera egomotion estimation)[11, 12].双目视觉测量是基于视差原理,由多幅图像获取物体三维信息的方法[13],由于与单目相机相比增加了景深信息,双目视觉自运动参数估计不仅能给出旋转参数和平移方向参数,还能给出单目相机所无法估计的平移的大小.

考虑两个相邻的成像时刻tktk+1,时间间隔为Δt.假设两时刻视野均包含的特征点个数为N.tk时刻载体速度和角速度分别为vkc和ωkc,tk+1时刻载体速度和角速度分别为vk+1c和ωk+1c,上标c表示摄像机坐标系.以下讨论均在tk时刻的摄像机坐标系下进行,为简便起见省略上标c.

摄像机坐标系的旋转和平移运动如图 1所示,其中OxcyczcO′x′cy′cz′c分别为tk时刻和tk+1的摄像机坐标系.

图 1 摄像机坐标系的运动 Fig. 1 Motion of the camera coordinate

记从tktk+1,摄像机平移向量为r,摄像机坐标系转动向量为θ,则对第i个特征点有[14]

其中,r=vkΔt;Ck+1k是从tk+1时刻摄像机坐标系到tk时刻摄像机坐标系的旋转矩阵.当Δt较小时,摄像机坐标系转过的角度θ为小角度,根据小角转动定理可知:
将r和Ck+1k代入式(1),可得
对于全部N个特征点,令
则有
从而可得到载体运动参数矩阵H的最小二乘解:

由于图像匹配精度有限,所以特征点视差存在误差,这导致景物位置Rc的计算亦有误差.景深误差与景深的关系为

式中,d为景深;dx为视觉传感器的像元物理尺寸;B为双目视觉测量基线长度;f为双目相机的焦距;δ(uL-uR)为双目视觉测量得到视觉的误差.

从式(9)可看出,景深的估计误差和景深的平方成正比.在式(8)中由景物位置Rc计算载体导航信息时,可以采用加权法,给不同景深的特征点赋予不同的权值,抑制较远的点带来过大的误差.根据加权最小二乘法的准则,设置加权阵为

其中Λ是N维列向量,其第i个元素为
矩阵H的计算公式为
由矩阵H中的各元素即可提取出载体的速度和角速度参数.

2 单独双目视觉导航算法

摄像机自运动参数估计得到的速度和角速度是其在摄像机坐标系下的投影,为了给出载体在导航坐标系下的导航信息,还需求出从摄像机坐标系到导航坐标系的转移矩阵.从摄像机系到导航系可通过两次旋转完成:首先通过摄像机安装矩阵,从摄像机系转到机体系,然后通过载体姿态矩阵,从机体系转到导航系.摄像机安装矩阵可以预先标定,而载体姿态矩阵则需要通过角速度积分实时计算.导航算法编排如图 2所示.

图 2 导航算法编排 Fig. 2 Layout of the navigation algorithm

首先通过摄像机姿态矩阵,将摄像机运动参数从摄像机坐标系转换到发射点惯性坐标系,然后进行积分计算载体的位置.与此同时,根据载体角速度求解载体姿态矩阵,确定载体姿态.根据刚体转动定理,载体姿态矩阵与载体角速度存在关系:

其中Ω是摄像机坐标系相对发射点坐标系(即世界坐标系)转动角速度的反对称矩阵:
由摄像机自运动估计得到的摄像机速度和角速度参数,通过图 2所示的计算流程,即可得到载体的位置和姿态估计值.

然而由于速度和角速度存在误差,通过积分得到的载体位姿误差会积累发散,因此有必要引入地标点所提供的绝对定位信息来抑制这一误差.

3 基于地标点修正的双目视觉自主导航方案设计

由前述分析可知,单独利用双目视觉测量得到的速度和角速度积分计算位姿,导航误差会累积发散.地标点匹配所提供的是绝对定位信息,这种方法精度高且误差不发散,然而该方法计算量大、鲁棒性较差,当视野内可见的地标点数量较少时无法正常导航.由此可以看出两种算法之间存在良好的互补性,因而拟采用组合导航的方法综合两者优点.

系统整体设计如图 3所示,视觉前端获取的图像一方面进行帧间匹配使用双目视觉导航的方式积分输出导航信息;另一方面,当视野内存在地标点时,通过地标点匹配的方式,结合电子地图获取地标点的绝对位置,随后在导航滤波器中对导航系统的误差状态进行估计,最后对误差进行补偿获取高精度的导航结果.

图 3 信息融合方案框图 Fig. 3 Scheme of the data fusion

图 3中还可以看出双目视觉导航与地标点匹配导航共享硬件资源,在简化系统结构的同时也降低了系统成本.

通过这样的组合,可以使系统在地标点稀疏或短时间内无法跟踪的情况下,持续稳定地完成导航工作,有利于提高系统的精度和可靠性.

3.1 系统状态模型

选择载体在世界坐标系下的位置误差以及摄像机三轴姿态误差作为系统状态向量:

导航坐标系选择为世界坐标系,也即发射点惯性坐标系.系统位置误差的导数是速度误差:
速度误差可视为白噪声处理.

姿态误差用计算摄像机坐标系与理想摄像机坐标系之间的转动角来表示.记导航坐标系到理想摄像机坐标系之间的转移矩阵为Cnc,导航坐标系到计算摄像机坐标系间的转移矩阵为C^nc.由于计算摄像机坐标系(c′)与理想摄像机坐标系(c)之间存在小角度转动:

其中,从而有
而摄像机相对导航坐标系的角速度可以表示为
亦即
其中
从而摄像机姿态误差角的微分方程为
根据刚体转动的欧拉定理,有
从而三轴角速度误差为
姿态角速度误差视为白噪声.

综上所述,系统的状态方程可以写为

其中
以上即为系统的状态方程.

3.2 系统量测模型

图 3所示,当前视野内成功识别地标点时,可获取量测信息,这时导航滤波器进行时间更新与量测更新,当识别失败,即视野内未发现地标点时,则只对导航滤波器进行时间更新.

当地标点识别成功时,可以在电子地图中查得其准确的地理坐标.此时选择以下两条途径计算所得的地标点在摄像机坐标系下的坐标之差作为观测量.其一为根据载体当前位姿与由地图获得的地标点地理坐标计算得到的地标点在摄像机坐标下的坐标;其二为使用双目摄像机立体测量得到的地标点在摄像机坐标系下的坐标.前者携带有载体当前位置和姿态估计的误差信息,而后者则包含着双目视觉系统的测量误差.为简化书写表达,引入下述记法:

假设某时刻,视野内可见的地标点有N个,在世界坐标系下坐标为Piw(i=1,2,…,N),相应的齐次坐标为iw,假设它在摄像机坐标系内的坐标为Jic.则

实际测量中,Cnc与rn是未知的,故需要通过双目视觉系统测量并计算Jic:

其中δJic表示双目视觉测量得到的地标点坐标与真实值的误差.

另一方面,根据当前摄像机位姿估计值,可计算地标点在摄像机坐标系下的位置,设为Eic:

式中,表示当前估计的摄像机姿态矩阵;表示当前估计的载体位置向量.根据前面的假设,其与系统误差状态之间存在如下关系:
将式(27)与式(29)作差得到:
由估计位姿与真实位姿之间的误差关系可知:
这里忽略了误差二阶小量.从而有
式(33)的左端可以通过测量与计算得到,而右端则是误差状态量的线性组合,且组合系数矩阵是完全可知的.从而可得系统的量测方程为
下标数字表示当前视野内地标点的序号.对于任意第i个地标点,Zi为三维列向量,其量测值为
对应的量测矩阵为
量测噪声为
由式(36)可清晰地看出,系统量测值为两部分之差,一部分是由地标点地理位置以及摄像机位姿参数估计值计算得到的地标点在摄像机坐标系下的坐标,另一部分是通过双目视觉系统测量得到的地标点在摄像机坐标系下的坐标.前者包含的是载体位姿估计误差,而后者的误差δJic则主要来源于双目视觉景深测量误差.下面对量测噪声δJic的统计特性进行分析.

对于平行光轴双目视觉系统,景深可由式(38)计算[15]:

对于某个特定的地标点,进一步可以计算出地标点在摄像机坐标系下的坐标:

从而双目视觉测量误差可以写为
误差的协方差阵为
其中
式(41)中σ2为图像特征点匹配误差的方差,这里假设匹配结果在图像两个方向上精度相同.而δJc即为量测噪声V.由于各个量测噪声之间是相关的,因此量测噪声阵R并非对角阵.而Kalman滤波要求量测噪声之间不相关,故需采用Cholesky分解的方法,对量测噪声阵进行分解变换[16],使其成为对角阵.

det(R)=8k12k32k52σ2>0为正定对称方阵,根据Cholesky分解定理可知,R可以唯一地被分解为

式中,D为正定对角阵;L是一个单位下三角矩阵.对于L,其逆矩阵存在且仍为单位下三角阵.记L的逆为M=L-1,在式(34)两端同时左乘M得到
式(44)即为新的量测方程.其中
并且有

通过上述变换,将系统量测噪声阵转化为对角阵D,消除了量测噪声互相关带来的影响.

4 仿真与验证 4.1 仿真试验

为对本文提出的算法进行仿真验证,设计了如图 4所示的仿真验证平台.平台主要包括轨迹发生模块、特征点/地标点生成模块、双目成像模块、视觉导航模块以及误差分析模块.仿真过程中,轨迹发生器首先生成标准轨迹;然后由特征点/地标点生成模块在轨迹沿途随机设置特征点和地标点;接着双目成像模块应用相机成像模型计算特征点/地标点的成像结果;再由视觉导航模块对成像结果进行处理,还原载体的导航信息;最后在误差分析模块中,对导航误差进行计算和分析.

图 4 仿真验证平台 Fig. 4 Scheme of the simulation platform

为验证地标点修正对提高视觉导航精度的作用,首先采用单独双目视觉导航算法对载体的位置进行估计;其次采用地标点修正的双目视觉导航算法对同一飞行过程进行导航解算.

仿真条件:飞行时长200s,无人机起飞点坐标为东经118°、北纬40°,初始方位角为45°,全程飞行高度在80~160m之间.机载双目摄像机焦距5mm,像素物理尺寸10-3mm,立体视觉测量基线2m,摄像机分辨率1024×1024.在仿真中,假设图像特征点匹配的精度能够达到0.1像素.每一帧画面内特征点的密度控制在50~70个左右,而地标点的密度约为3~5个左右.地标点的位置误差标准差为1m.考虑到视觉导航图像处理运算量较大,仿真的时间更新频率设置为1Hz.

4.2 仿真结果

在上述仿真条件下,单独双目视觉导航和采用地标点修正的双目视觉导航算法的导航结果在东、北、天3个方向的误差曲线如图 5,图 6,图 7所示.

图 5 东向位置误差 Fig. 5 Positioning error (East)

图 6 北向位置误差 Fig. 6 Positioning error (North)

图 7 高度位置误差 Fig. 7 Positioning error (Height)

图 5,图 6,图 7中可以看出,由于单独双目视觉导航算法是一种相对导航方式,其误差具有积累发散的趋势,而图 7表明在高度方向的误差更为显著.这是因为在导航过程中,特征点分布于地面,从双目视觉测量原理可知,景深变化相对于图像平移更难测量,故而在高度方向的导航精度较差.而地标点修正的双目视觉导航算法,由于将地标点所提供的绝对导航信息引入了导航系统,故可以对系统的误差进行估计并修正,从而抑制了导航误差的积累,提高了系统长时间工作时的导航精度.从图中可以看出,经过地标点修正后,导航结果保持在较高的精度,并且不随积累而发散.

5 结 论

通过将地标点匹配与双目视觉导航结合起来,有效地解决了双目视觉导航误差发散的问题,提出了一种适用于无人机、智能机器人及智能车辆等设备的高精度视觉导航系统设计方案.该方案是一种绝对/相对定位相结合的导航方式:一方面利用绝对导航误差不发散的优点,对相对定位结果进行修正以确保导航结果的精度;另一方面,发挥相对导航自主性强的优点,弥补了绝对导航对先验信息依赖性强的缺点,当短时间内视野中无可识别地标点时,组合导航系统仍能继续工作.同时,两种导航方式共用一套硬件系统,在保证性能的同时降低了系统成本.仿真结果表明这种组合导航方法具有成本低、精度高、自主性强、导航信息完备的优点.

参考文献
[1] Ribas D,Ridao P,Neira J.Underwater SLAM for structured environments using an imaging sonar[M].Berlin:Springer,2010
[2] Lee D,Kim G,Kim D,et al.Vision-based object detection and tracking for autonomous navigation of underwater robots[J].Ocean Engineering,2012,48:59-68
Click to display the text
[3] Sasiadek J Z,Walker M J.Vision-based UAV navigation[C]//AIAA Guidance,Navigation and Control Conference and Exhibit.Reston,VA:AIAA,2008:18-21
Click to display the text
[4] 介鸣,黄显林.基于月貌匹配的视觉导航方法[J].哈尔滨工程大学学报,2007,28(1):11-30 Jie Ming,Huang Xianlin.Vision navigation based on lunar terrain matching[J].Journal of Harbin Engineering University,2007,28(1):11-30(in Chinese)
Cited By in Cnki (19)
[5] Isher M K,Xu H,Long H.Robustness towards application of multi-objective optimisation for autonomous off-road vehicle on rough terrain[J].International Journal of Mechatronics and Aut- omation,2012,2(2):103-111
Click to display the text
[6] Chatterjee A,Rakshit A,Singh N N.Vision-based mobile robot navigation using subgoals[M].Berlin:Springer,2013:47-82
[7] 黄显林,姜肖楠,卢鸿谦,等.自主视觉导航方法综述[J].吉林大学学报:信息科学版,2010(2):158-165 Huang Xianlin,Jiang Xiaonan,Lu Hongqian,et al.Survey of vision for autonomous navigation[J].Journal of Jilin University:Information Science Edition,2010(2):158-165(in Chinese)
Cited By in Cnki (18)
[8] Li X,Wang J.Image matching techniques for vision-based indoor navigation systems: performance analysis for 3D map based approach[C]//2012 International Conference on Indoor Positioning and Indoor Navigation, IPIN 2012-Conference Proceedings.Washington,DC:IEEE Computer Society,2012:1-8
[9] Engelhard N,Endres F,Hess J,et al.Real-time 3D visual SLAM with a hand-held RGB-D camera[C]//Proc of the RGB-D Workshop on 3D Perception in Robotics at the European Robotics Forum,2011
[10] 李宇波,朱效洲,卢惠民,等.视觉里程计技术综述[J].计算机应用研究,2012,29(8):2801-2805 Li Yubo,Zhu Xiaozhou,Lu Huimin,et al.Review on visual odometry technology[J].Application Research of Computers,2012,29(8):2801-2805(in Chinese)
Cited By in Cnki (2)
[11] Burger W,Bhanu B.Estimating 3-D egomotion from perspective image sequences[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1990,12(11):1040-1058
Click to display the text
[12] Perrone J A.A closer look at the visual input to self-motion estimation[J].Motion Vision-Computational,Neural,and Ecological Constraints,2001:169-179
Click to display the text
[13] 徐德,谭民,李原.机器人视觉测量与控制[M].北京:国防工业出版社,2011 Xu De,Tan Min,Li Yuan.Visual measurement and control for robots[M].Beijing:National Defense Industry Press,2011(in Chinese)
[14] Kelly J,Saripalli S,Sukhatme G S.Combined visual and inertial navigation for an unmanned aerial vehicle[C]//Springer Tracts in Advanced Robotics.Heidelberg:Springer Verlag,2008,42:255-264
Click to display the text
[15] 张广军.视觉测量[M].北京:科学出版社,2008 Zhang Guangjun.Visual measurement[M].Beijing:Science Press,2008(in Chinese)
[16] 段战胜,韩崇昭.相关量测噪声情况下多传感器集中式融合跟踪[J].系统工程与电子技术,2005,27(7):1160-1163 Duan Zhansheng,Han Chongzhao.Multi-sensor centralized fusion tracking with correlated measurement noises[J].Systems Engineering and Electronics,2005,27(7):1160-1163(in Chinese)
Cited By in Cnki (7)
http://dx.doi.org/10.13700/j.bh.1001-5965.2013.0588
北京航空航天大学主办。
0

文章信息

张洋, 王新龙
Zhang Yang, Wang Xinlong
一种地标点修正的高精度双目视觉导航方法
Landmark fixed high-precision binocular visual navigation method
北京航空航天大学学报, 2014, 40(9): 1305-1311
Journal of Beijing University of Aeronautics and Astronsutics, 2014, 40(9): 1305-1311.
http://dx.doi.org/10.13700/j.bh.1001-5965.2013.0588

文章历史

收稿日期:2013-10-15
网络出版日期: 2014-03-18

相关文章

工作空间