视觉里程计中的相机姿态和高度实时测量方法

视觉定位方法， 该方法通过提取和跟踪全景图像中
为提高视觉定位精度， 目前最常使用的相机姿
昂贵且工作前准备时间较长， 后者易受卫星信号干 扰或遮挡的影响。此外， 在动态实时获得相机高度 的研究方面， 目前还未见有效的测量方法的报道。 为此， 本文提出了基于正交二维激光扫描的动态条 件下相机姿态和高度测量方法
图1
相机姿态和高度测量原理示意图
2
姿态和高度解算公式
由于本文方法是以道路平面作为参考基准来
获取相机外参数据， 因此首先需要由两条垂直相交 的路面剖面直线得到道路平面的表达式方程， 再在 此基础上推导出相机的姿态和高度计算公式。 2.1 假设经 RANSAC 算法获得的道面剖线方程分 道路平面方程的推导
的中点。由式 （9） 可得到公垂线段 MN 中点 K 的坐 标为：
K(
ìM (b t , 0, - c1 - a t ) 1 1 1 1 ï b1 ï （9） í c2 ï N (0, b t , a t ) ï 2 2 2 2 b2 î 令道路平面通过异面直线 L1 和 L 2 的公垂线段 c1 b1 + c 2 b 2 + a1 t1 + a 2 t 2 b1 t1 b 2 t 2 , , ) （10） 2 2 2 又有公垂线的方向向量 {A， B， C} 为道路平面的
别为：
{
图2
相机姿态和高度测量方法流程图
其中系数 a1, b1, c1, a 2 , b 2 , c 2 已通过 RANSAC 算法估计 得到。
L1:a1 X + b1 Z + c1 = 0, Y = 0 L 2 :a 2Y + b 2 Z + c 2 = 0, X = 0
（1）
1356
传 感 技 术 学 报 www.chinatransducers.com
修改日期： 2015-07-09
第9期
曹
毓， 张小虎等： 视觉里程计中的相机姿态和高度实时测量方法
1355
机外参的实时精确获取是视觉里程计实现高精度 定位的前提， 这也是当前视觉里程计研究的热点问 题之一： 文献 ［7］ 提出了 MVO/SINS 导航方案， 将单 目视觉里程计和惯性导航系统组合使用， 得到了
（1.College of Aerospace Science and Engineering， National University of Defense Technology ， Changsha 410073， China；
CAO Yu1*， ZHANG Xiaohu1， FENG Ying2
ì X - x1 Y - y1 Z - z1 L: = = ï n1 p1 ï 1 m1 （2） í X-x Y-y Z - z2 2 2 ï = = ïL 2 : m2 n2 p2 î 则公垂线 LC 的方程为： X - x1 - m1 t1 Y - y1 - n1 t1 Z - z1 - p1 t1 LC : = = （3） A B C 令 LC 与 L1 和 L 2 交点分别为 M 和 N， 其坐标为：
。该方法以道路
平面法线方向而非传统的重力场方向作为姿态测 量参考基准， 不但得到的姿态数据更有利于提高视 觉定位的精度， 而且可同时求解出相机的高度。室 内外实验结果均表明： 本文方法在保证测量精度的 同时， 具有测量频率较高、 环境适应性强、 不受车辆 加速影响等优点。
基于正交二维激光扫描的相机姿态和高度测
关键词： 计算机视觉； 视觉里程计； 激光扫描； 姿态测量； 高度测量 中图分类号： TP393 文献标识码： A 文章编号： 1004-1699(2015)09-1354-07 视觉里程计还具有结构简单、 便于系统集成等优 点。然而实际应用中发现： 视觉定位的精度敏感于 相机内外参数标定的效果［3］， 尤其是平台运动过程 中难免的震动和颠簸会使得相机外参 （姿态和高 度） 随机变化， 这会导致显著的定位误差。因此相
第 28 卷 第 9 期 2015 年 9 月
传 感 技 术 学 报
CHINESE JOURNAL OF SENSORS AND ACTUATORS
Vol������ 28
Sept. 2015
No. 9
Real-Time Measurement of Camera Attitude and Height in Visual Odometer*
成本过高， 要么精度无法满足要求， 为此提出了基于路面激光扫描的相机外参数实时测量方法。该方法将两台二维激光扫描 仪相互正交安装且向下扫描， 对获得的沿着两个方向的路面扫描线使用 RANSAC 算法估计出直线方程， 根据两直线方程求得 道路平面方程， 并以该平面为参考获得相机相对路面的姿态和高度数据。室内实验结果表明： 静态条件下对姿态的测量误差 最大约 0.1°， 高度测量误差最大 6 mm； 室外动态实验结果表明： 与传统的惯性测量方法不同， 相机外参数测量结果不受车辆加 减速运动的影响， 且其动态姿态测量精度明显高于精度为 1°的惯性测量系统。由于该方法获得的姿态和高度数据是以道路平 面为参考基准， 尤其适用于单目视觉里程计中以辅助提高定位精度。
［10-11］
程。在获得道路平面方程后， 以道路平面的法线方 向为参考可获得相机姿态， 通过计算 P 点到道路平 面的距离可获得相机高度。在一般的应用背景下， 即使路面局部存在破损， 其整体平坦的特性也不会 改变， 这为采用 RANSAC 算法提取路面剖面直线方 法的可行性提供了保障。 量方法的流程如图 2 所示。
法线向量， 可确定扫描仪系统坐标系 CS- scanner 下 （其 定义如图 1 的 X-Y-Z 所示） 道路平面方程 （点法式 形式表示） 如下：
1
相机姿态和高度测量原理
图 1 为基于正交二维激光扫描的相机姿态和高
［8］ 提出了基于全景相机和 “视觉罗盘” 原理的单目 标志特征实现运动平台航向的实时校正， 提高了定 位精度。 态测量手段为惯性测量系统［7］或者 GPS［9］， 前者价格
1 421 m 行驶距离内误差 4% 的定位结果； 此外文献
使用相机实现运动平台位置解算的方法称为 视觉定位法或视觉测程法
［1-2］
应产品称为视觉里程计 （Visual Odometer） 。它比
［3-6］
（Visual Odometry）Fra Baidu bibliotek， 相
惯性导航系统成本低廉， 更利于推广应用； 与码盘 里程计相比不会因车轮打滑而引入定位误差； 此外
项目来源： 博士后基金项目 （2014m562649） 收稿日期： 2015-05-05 ————————————
2.College of Optoelectronic Science and Engineering ， National University of Defense Technology ， Changsha 410073， China）
Abstract： In the application of visual odometer， acquiring the high- precision attitude and height of camera in real time helps to improve the visual positioning accuracy. But existing solutions are either expensive or low in preci⁃ sion， so we bring forward an real-time method to measure the camera’ s external parameters based on the laser scan⁃ ning of road surface. Two 2D laser scanner orthogonal installed and downward scan ， then the linear equation was es⁃ timated using the RANSAC algorithm. After that ， road plane equation is obtained， which is the reference of the cam⁃ era’ s attitude and altitude data. the indoor experiment results showed that ： in the static conditions， the measure⁃ ment error of the attitude is about 0.1 degree， maximum height measurement error is about 6 mm； the outdoor dy⁃ namic experimental results showed that： different from the traditional method of inertial measurement ， camera exter⁃ nal parameters measurement results are not affected by the vehicle deceleration influence ， and the attitude measure⁃ ment precision is significantly higher than the inertial measurement system which has accuracy of 1 degree. Owing to obtaining the attitude and altitude data as the road plane for reference ， the method is especially suitable for assist monocular visual odometry to improving the positioning accuracy. Key words： computer vision； visual odometer； laser scanning； attitude measurement； height measurement EEACC： 7230 doi： 10.3969/j.issn.1004-1699.2015.09.015
第 28 卷
由于直线 L1 和 L 2 是扫描仪 S1 和 S 2 的扫描平面 与道面的交线， 因此在理想情况下根据直线 L1 和 L 2 的方程可得道路平面的表达式方程。然而由于 的影响，L1 和 L 2 往往为异面直线。此时可令道路 RANSAC 算法的估计误差和扫描仪测距误差等因素 平面通过二者公垂线段的中点， 公垂线方向为平面 法线方向， 进而解出平面方程。 L1 和 L 2 的公垂线 方程可通过异面直线的公垂线公式求得［12］。 设异面直线 L1 和 L 2 的方程分别为：
视觉里程计中的相机姿态和高度实时测量方法*
（1. 国防科学技术大学航天科学与工程学院， 长沙 410073； 2. 国防科学技术大学光电科学与工程学院， 长沙 410073）
曹
毓 1*， 张小虎 1， 冯
莹2
摘
要： 在视觉里程计的应用中， 实时准确的获得相机姿态和高度数据有助于提高视觉定位的精度。而现有解决方案要么
度测量方法原理示意图。图中 X- Y- Z 为扫描仪系 统坐标系 CS- scanner， X 轴和 Y 轴平行于路面， Z 轴垂直
于路面且向上为正向。两台二维激光扫描仪 S1 和 S2 相互正交放置且扫描平面平行于 Z 轴向下扫描路 面。 P 为相机的安装位置点， p 点为 P 在道路平面上 的投影点， 显然线段 Pp 垂直于道路平面， 其长度即 为相机的高度。在一个扫描周期内由扫描仪 S1 和 S2 可得到两条路面扫描线， 利用 RANSAC 算法对其实 方程。由于 L1 和 L2 为路面剖面直线， 根据 L1 和 L2 的 施直线估计， 分别得到对应的直线 L1 和 L2 的表达式 方程可得道路平面在 X- Y- Z 坐标系下的表达式方