视觉导航与卫星导航、惯性导航、地磁导航等相比具有结构简单、适应性强、相互干扰小、成本低和环境信息丰富等优点.视觉导航技术在许多研究领域得到了重视和发展,其应用范围涵盖了海、陆、空、天等环境,如海洋环境下基于视觉海底拼图和水下标记的视觉导航[1, 2];空天环境下的无人机视觉导航[3]、月面着陆的视觉导航[4]等应用;陆地环境下的智能移动车辆[5]、移动机器人系统[6]等都大量使用了视觉导航技术,它已经成为提高机械智能化水平必不可少的系统装备技术,因此视觉导航技术的研究具有重要意义.
传统视觉导航根据地图的使用情况,可分为3类[7]:基于地图的导航方法、地图建立型导航方法以及无地图导航方法.基于地图的导航方法通过将视觉传感器获得的图像与预先存储的地图进行匹配,获取当前载体的位姿信息,这种方法精度高但运算量大、实时性差[8].地图建立型导航方法可以在线建立载体附近小范围内的地图,用于导航与定位,这种方法对陌生环境具有良好的适应性,但是导航过程缓慢,仅适用于低速机器人小范围内的导航等[9].无地图导航方法不需要对环境信息进行全面描述.光流法、基于特征跟踪或外观的导航方法是无地图视觉导航方法的主要研究方向.这类导航方法运算量较小、实时性好,但由于缺少绝对参考,导航误差会积累发散[10].
为了解决无地图导航方法中导航误差会积累发散的问题,本文结合摄像机自运动参数估计理论,提出了一种利用地标点修正的高精度双目视觉导航方法.通过对地貌特征点进行跟踪,估计载体的运动参数,同时为了限制误差的积累发散,引入地标点信息对导航误差予以修正.该方法不需要人为设置导航特征,运动参数的提取完全来自地貌特征点,具有较强的自主性.同时只需预先在电子地图上标定一些特征点用于导航误差校正,避免了复杂的地图匹配运算,精度高且运算量小、实时性好.
1 基于双目视觉的摄像机自运动估计原理
从视频图像序列中获取摄像机运动信息的过程即为摄像机自运动估计(camera egomotion estimation)[11, 12].双目视觉测量是基于视差原理,由多幅图像获取物体三维信息的方法[13],由于与单目相机相比增加了景深信息,双目视觉自运动参数估计不仅能给出旋转参数和平移方向参数,还能给出单目相机所无法估计的平移的大小.
考虑两个相邻的成像时刻tk和tk+1,时间间隔为Δt.假设两时刻视野均包含的特征点个数为N.tk时刻载体速度和角速度分别为vkc和ωkc,tk+1时刻载体速度和角速度分别为vk+1c和ωk+1c,上标c表示摄像机坐标系.以下讨论均在tk时刻的摄像机坐标系下进行,为简便起见省略上标c.
摄像机坐标系的旋转和平移运动如图 1所示,其中Oxcyczc和O′x′cy′cz′c分别为tk时刻和tk+1的摄像机坐标系.
记从tk到tk+1,摄像机平移向量为r,摄像机坐标系转动向量为θ,则对第i个特征点有[14]
由于图像匹配精度有限,所以特征点视差存在误差,这导致景物位置Rc的计算亦有误差.景深误差与景深的关系为
从式(9)可看出,景深的估计误差和景深的平方成正比.在式(8)中由景物位置Rc计算载体导航信息时,可以采用加权法,给不同景深的特征点赋予不同的权值,抑制较远的点带来过大的误差.根据加权最小二乘法的准则,设置加权阵为
2 单独双目视觉导航算法
摄像机自运动参数估计得到的速度和角速度是其在摄像机坐标系下的投影,为了给出载体在导航坐标系下的导航信息,还需求出从摄像机坐标系到导航坐标系的转移矩阵.从摄像机系到导航系可通过两次旋转完成:首先通过摄像机安装矩阵,从摄像机系转到机体系,然后通过载体姿态矩阵,从机体系转到导航系.摄像机安装矩阵可以预先标定,而载体姿态矩阵则需要通过角速度积分实时计算.导航算法编排如图 2所示.
首先通过摄像机姿态矩阵,将摄像机运动参数从摄像机坐标系转换到发射点惯性坐标系,然后进行积分计算载体的位置.与此同时,根据载体角速度求解载体姿态矩阵,确定载体姿态.根据刚体转动定理,载体姿态矩阵与载体角速度存在关系:
然而由于速度和角速度存在误差,通过积分得到的载体位姿误差会积累发散,因此有必要引入地标点所提供的绝对定位信息来抑制这一误差.
3 基于地标点修正的双目视觉自主导航方案设计
由前述分析可知,单独利用双目视觉测量得到的速度和角速度积分计算位姿,导航误差会累积发散.地标点匹配所提供的是绝对定位信息,这种方法精度高且误差不发散,然而该方法计算量大、鲁棒性较差,当视野内可见的地标点数量较少时无法正常导航.由此可以看出两种算法之间存在良好的互补性,因而拟采用组合导航的方法综合两者优点.
系统整体设计如图 3所示,视觉前端获取的图像一方面进行帧间匹配使用双目视觉导航的方式积分输出导航信息;另一方面,当视野内存在地标点时,通过地标点匹配的方式,结合电子地图获取地标点的绝对位置,随后在导航滤波器中对导航系统的误差状态进行估计,最后对误差进行补偿获取高精度的导航结果.
从图 3中还可以看出双目视觉导航与地标点匹配导航共享硬件资源,在简化系统结构的同时也降低了系统成本.
通过这样的组合,可以使系统在地标点稀疏或短时间内无法跟踪的情况下,持续稳定地完成导航工作,有利于提高系统的精度和可靠性.
3.1 系统状态模型
选择载体在世界坐标系下的位置误差以及摄像机三轴姿态误差作为系统状态向量:
姿态误差用计算摄像机坐标系与理想摄像机坐标系之间的转动角来表示.记导航坐标系到理想摄像机坐标系之间的转移矩阵为Cnc,导航坐标系到计算摄像机坐标系间的转移矩阵为C^nc.由于计算摄像机坐标系(c′)与理想摄像机坐标系(c)之间存在小角度转动:
综上所述,系统的状态方程可以写为
3.2 系统量测模型
如图 3所示,当前视野内成功识别地标点时,可获取量测信息,这时导航滤波器进行时间更新与量测更新,当识别失败,即视野内未发现地标点时,则只对导航滤波器进行时间更新.
当地标点识别成功时,可以在电子地图中查得其准确的地理坐标.此时选择以下两条途径计算所得的地标点在摄像机坐标系下的坐标之差作为观测量.其一为根据载体当前位姿与由地图获得的地标点地理坐标计算得到的地标点在摄像机坐标下的坐标;其二为使用双目摄像机立体测量得到的地标点在摄像机坐标系下的坐标.前者携带有载体当前位置和姿态估计的误差信息,而后者则包含着双目视觉系统的测量误差.为简化书写表达,引入下述记法:
假设某时刻,视野内可见的地标点有N个,在世界坐标系下坐标为Piw(i=1,2,…,N),相应的齐次坐标为iw,假设它在摄像机坐标系内的坐标为Jic.则
实际测量中,Cnc与rn是未知的,故需要通过双目视觉系统测量并计算Jic:
另一方面,根据当前摄像机位姿估计值,可计算地标点在摄像机坐标系下的位置,设为Eic:
对于平行光轴双目视觉系统,景深可由式(38)计算[15]:
对于某个特定的地标点,进一步可以计算出地标点在摄像机坐标系下的坐标:
因det(R)=8k12k32k52σ2>0为正定对称方阵,根据Cholesky分解定理可知,R可以唯一地被分解为
通过上述变换,将系统量测噪声阵转化为对角阵D,消除了量测噪声互相关带来的影响.
4 仿真与验证 4.1 仿真试验
为对本文提出的算法进行仿真验证,设计了如图 4所示的仿真验证平台.平台主要包括轨迹发生模块、特征点/地标点生成模块、双目成像模块、视觉导航模块以及误差分析模块.仿真过程中,轨迹发生器首先生成标准轨迹;然后由特征点/地标点生成模块在轨迹沿途随机设置特征点和地标点;接着双目成像模块应用相机成像模型计算特征点/地标点的成像结果;再由视觉导航模块对成像结果进行处理,还原载体的导航信息;最后在误差分析模块中,对导航误差进行计算和分析.
为验证地标点修正对提高视觉导航精度的作用,首先采用单独双目视觉导航算法对载体的位置进行估计;其次采用地标点修正的双目视觉导航算法对同一飞行过程进行导航解算.
仿真条件:飞行时长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,图 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) |