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

曹　毓，张小虎，冯　莹

（1.国防科学技术大学航天科学与工程学院，长沙410073；2.国防科学技术大学光电科学与工程学院，长沙410073）

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

曹毓1*，张小虎1，冯莹2

（1.国防科学技术大学航天科学与工程学院，长沙410073；2.国防科学技术大学光电科学与工程学院，长沙410073）

在视觉里程计的应用中，实时准确的获得相机姿态和高度数据有助于提高视觉定位的精度。而现有解决方案要么成本过高，要么精度无法满足要求，为此提出了基于路面激光扫描的相机外参数实时测量方法。该方法将两台二维激光扫描仪相互正交安装且向下扫描，对获得的沿着两个方向的路面扫描线使用RANSAC算法估计出直线方程，根据两直线方程求得道路平面方程，并以该平面为参考获得相机相对路面的姿态和高度数据。室内实验结果表明:静态条件下对姿态的测量误差最大约0.1°，高度测量误差最大6 mm；室外动态实验结果表明:与传统的惯性测量方法不同，相机外参数测量结果不受车辆加减速运动的影响，且其动态姿态测量精度明显高于精度为1°的惯性测量系统。由于该方法获得的姿态和高度数据是以道路平面为参考基准，尤其适用于单目视觉里程计中以辅助提高定位精度。

计算机视觉；视觉里程计；激光扫描；姿态测量；高度测量

EEACC:7230doi:10.3969/j.issn.1004-1699.2015.09.015

使用相机实现运动平台位置解算的方法称为视觉定位法或视觉测程法［1-2］（Visual Odometry），相应产品称为视觉里程计［3-6］（Visual Odometer）。它比惯性导航系统成本低廉，更利于推广应用；与码盘里程计相比不会因车轮打滑而引入定位误差；此外视觉里程计还具有结构简单、便于系统集成等优点。然而实际应用中发现:视觉定位的精度敏感于相机内外参数标定的效果［3］，尤其是平台运动过程中难免的震动和颠簸会使得相机外参（姿态和高度）随机变化，这会导致显著的定位误差。因此相机外参的实时精确获取是视觉里程计实现高精度定位的前提，这也是当前视觉里程计研究的热点问题之一:文献［7］提出了MVO/SINS导航方案，将单目视觉里程计和惯性导航系统组合使用，得到了1 421 m行驶距离内误差4%的定位结果；此外文献［8］提出了基于全景相机和“视觉罗盘”原理的单目视觉定位方法，该方法通过提取和跟踪全景图像中标志特征实现运动平台航向的实时校正，提高了定位精度。

为提高视觉定位精度，目前最常使用的相机姿态测量手段为惯性测量系统［7］或者GPS［9］，前者价格昂贵且工作前准备时间较长，后者易受卫星信号干扰或遮挡的影响。此外，在动态实时获得相机高度的研究方面，目前还未见有效的测量方法的报道。为此，本文提出了基于正交二维激光扫描的动态条件下相机姿态和高度测量方法［10-11］。该方法以道路平面法线方向而非传统的重力场方向作为姿态测量参考基准，不但得到的姿态数据更有利于提高视觉定位的精度，而且可同时求解出相机的高度。室内外实验结果均表明:本文方法在保证测量精度的同时，具有测量频率较高、环境适应性强、不受车辆加速影响等优点。

1　相机姿态和高度测量原理

图1为基于正交二维激光扫描的相机姿态和高度测量方法原理示意图。图中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坐标系下的表达式方程。在获得道路平面方程后，以道路平面的法线方向为参考可获得相机姿态，通过计算P点到道路平面的距离可获得相机高度。在一般的应用背景下，即使路面局部存在破损，其整体平坦的特性也不会改变，这为采用RANSAC算法提取路面剖面直线方法的可行性提供了保障。

基于正交二维激光扫描的相机姿态和高度测量方法的流程如图2所示。

图1　相机姿态和高度测量原理示意图

图2　相机姿态和高度测量方法流程图

2　姿态和高度解算公式

由于本文方法是以道路平面作为参考基准来获取相机外参数据，因此首先需要由两条垂直相交的路面剖面直线得到道路平面的表达式方程，再在此基础上推导出相机的姿态和高度计算公式。

2.1道路平面方程的推导

假设经RANSAC算法获得的道面剖线方程分别为:

其中系数a1，b1，c1，a2，b2，c2已通过RANSAC算法估计得到。

由于直线L1和L2是扫描仪S1和S2的扫描平面与道面的交线，因此在理想情况下根据直线L1和L2的方程可得道路平面的表达式方程。然而由于RANSAC算法的估计误差和扫描仪测距误差等因素的影响，L1和L2往往为异面直线。此时可令道路平面通过二者公垂线段的中点，公垂线方向为平面法线方向，进而解出平面方程。L1和L2的公垂线方程可通过异面直线的公垂线公式求得［12］。

设异面直线L1和L2的方程分别为:

则公垂线LC的方程为:

令LC与L1和L2交点分别为M和N，其坐标为:

在式（2）～式（4）中:

为利用异面直线的公垂线定理，将式（1）中直线L1和L2的方程改写为:

对比式（2）和式（6），可得t1和t2分别为:

结合式（3），可得直线L1和L2的公垂线LC的方程为:

LC与L1和L2的交点M和N的坐标为:

令道路平面通过异面直线L1和L2的公垂线段的中点。由式（9）可得到公垂线段MN中点K的坐标为:

又有公垂线的方向向量｛A，B，C｝为道路平面的法线向量，可确定扫描仪系统坐标系CS-scanner下（其定义如图1的X-Y-Z所示）道路平面方程（点法式形式表示）如下:

2.2相机姿态的解算

相机姿态角是以扫描仪系统坐标系CS-scanner下道路平面的法线向量为参考基准获得的。不妨令路面坐标系为CG-plane，且该坐标系的原点与CS-scanner的原点重合。CG-plane与通常意义下的世界坐标系CG不同，主要区别在于CG-plane中Z轴垂直于道路平面，而CG中Z轴方向一般与重力加速度方向重合。两个坐标系的差别视道路平面在世界坐标系CG下的倾斜程度而定，当道面水平时二者无区别。令:

CG-plane=RGS-scannerCS-scanner（12）

其中RGS-scanner为扫描仪系统坐标系到路面坐标系的变换矩阵，坐标轴旋转次序约定为X→Y→Z，不考虑相机航向角，RGS-scanner可表示为:

其中φ和θ分别为相机的俯仰角和横滚角。之所以不考虑航向角，是因为视觉里程计本质上属于相对定位方法，虽然它可以解算出航向信息，但是长时间定位后该航向会出现显著漂移［3］。因此现有的视觉定位法大多利用GPS等其他手段［7-9］获得真实的航向数据以提高长时间定位的精度。

由于式（12）中未知数有两个，只需要知道任意一个三维坐标点Q分别在CG-plane与CS-scanner坐标系下的坐标，即可由式（12）和式（13）求出φ和θ。针对该问题，我们提出并采用了“单位球相交”的方法，获得了坐标系旋转前后Q点的坐标映射关系。该方法原理如图3所示。

图3　单位球相交法计算姿态的原理示意图

以坐标系CG-plane和CS-scanner的公共原点O为中心作一个半径为1的单位半球，分别与两个坐标系的Z轴和ZG-plane轴相交于点Q和′，可知Q和′在各自坐标系中的坐标均为（0，0，-1）。只需知道点′在CS-scanner坐标系下的坐标值 （x，y，z）即可解出RGS-scanner，进而求得ϕ和θ。此时有:

由前文的分析知:ZG-plane轴与两条路面剖线的公垂线LC平行，由式（8）知LC的方向向量为｛A，B，C｝，因此ZG-plane轴在CS-scanner坐标系下的直线方程为:

联立ZG-plane轴和单位半球面的方程:

最终解得点Q′在CS-scanner坐标系下的坐标值，公式中sign为符号函数:

将式（14）的矩阵形式展开，得到:

由式（18）的1式，得:

由式（18）的2式×cosθ+3式×sinθ，得:

θ=asin（x）（20）

将式（17）的结果分别代入式（19）和式（20），得到φ和θ:

2.3相机高度的解算

设相机安装点P在扫描仪系统坐标系CS-scanner下的坐标为（xC，yC，zC），由式（11）给出的道路平面方程结合点到平面的距离公式，可算得相机距离道路平面的高度HCam.为:

3　实验结果与分析

实验中使用了两台德国SICK公司的二维激光扫描仪（型号:LMS291-S05），最大扫描频率75 Hz，测距量程80 m。利用星网宇达公司的惯性姿态测量系统ADU5600来检验方法的角度测量精度，其姿态测量频率为100 Hz，标称静态姿态测量精度0.2°，动态精度1°。实验设备架设方式如图4所示，分别在室内静态和室外动态条件下测试了方法的精度。

图4　实验设备架设方式

3.1室内静态实验

室内静态实验场景如图5所示。由于室内的地面为水平面，此时扫描仪系统坐标系CS-scanner与世界坐标系CG可重合，为此图中采用重力锤获得了相机安装点P在地面的垂直投影点p，用激光测距机测量Pp的长度即得P点真实高度值。

图5　室内静态实验场景

图6给出了实验中两台正交架设的扫描仪一次同步扫描的扫描点分布图，图中两条直线为使用RANSAC算法估计得到的路面剖线L1和L2。

实验共实施6组。为检验算法对高度的测量精度，使用莱卡公司DISTO-A5型激光测距机（测距精度1.5 mm）测量了每组实验中对应的相机安装点P的实际高度值。实验详细结果如表1所示。表1中每组数据均为一次实验中静态多次测量平均值，可见6组实验中对姿态的测量误差最大约0.1°，高度测量误差最大6 mm。

实验结果误差的主要原因应该是扫描仪的安装标定精度不够所致。此外设备自身精度限制也是导致存在误差的重要因素:由于ADU5600姿态测量系统的静态精度为0.2°，有必要使用更高精度的设备来进一步检验方法的姿态测量精度。

图6　扫描点分布及RANSAC估计出的直线

表1　静态实验结果数据

3.2室外动态实验

为验证方法在实际应用中的效果，选择三轮车作为移动搭载平台，在一平坦开阔场地实施室外的动态实验，实验设备安装方式如图7所示。

图7　室外实验设备实物

为验证车辆颠簸状态下本文方法的姿态和高度测量精度，在路面设置了5个障碍物，其相对位置摆放如图8所示。图8中1～4号障碍物放置于三轮车的后轮行驶轨迹上，用以产生车辆沿横滚方向的颠簸；5号障碍物置于前轮行驶轨迹上，用以产生车辆沿俯仰方向的颠簸。

图8　室外动态实验场景

实验中得到的车辆俯仰角、横滚角及相机高度变化曲线分别如图9～图11所示。可明显看出两种方法获得的姿态测量结果曲线的差别。

图9　俯仰角变化曲线

图10　横滚角变化曲线

图11　相机高度变化曲线

图9、图10和图11中的数字1～5分别指示了对应序号的路面障碍物引发车辆颠簸所导致的姿态和高度曲线的变化。图9中字母A到E代表车辆的五个状态阶段:A为车辆静止、驾驶员在车下阶段，B为车辆静止、驾驶员在车上阶段，C为车辆运动阶段，D为车辆停止、驾驶员在车上阶段，E为车辆停止、驾驶员在车下阶段。可见人的上下车均对应了车辆俯仰角的变化。由图9中的阶段E发现，车辆虽已处于静止状态，但是ADU的俯仰角测量结果明显出现了向上漂移，这是由于车辆突然减速停止导致姿态传感器出现测量误差引起的。相反的，本文方法测量结果不受车辆运动状态影响。同时观察图9、图10和图11的曲线波动情况发现:车辆横滚角变化主要受1～4号障碍物的影响，而5号障碍物主要影响了车辆俯仰角的变化；相机的高度在车辆每经过障碍物时均发生较大变化。因此，以上实验结果均与实际的实验情况吻合。

由于实验中用以对比验证精度的姿态传感器本身精度较低，无法准确定量给出本文方法的姿态测量精度，但是由结果数据结合实验条件不难得出结论，本方法的姿态测量精度明显高于ADU5600姿态测量系统。此外，由于作者未寻找到能实现动态条件下平台高度测量的有效方法或设备，因此方法在动态条件下的高度解算精度暂无法得到验证。

4　结论

本文研究了基于正交二维激光扫描的动态平台姿态和高度测量方法。首先详细分析了该方法的原理和理论公式推导过程，然后通过实验对该方法的测量精度加以验证。室内外的实验结果表明:该方法对相机姿态和高度的测量精度高、动态响应速度快，测量结果不受车辆加减速运动的影响，其姿态测量精度明显高于动态精度为1°的ADU5600姿态测量系统。

然而，本文工作在如下几个方面还有待改进和进一步完善，具体如下:

首先，受硬件水平及实验测试条件等限制，实验中并没有准确得到本文方法精度的定量结果，进一步的高精度实验验证工作有待深入开展。比如:对扫描仪的安装实施更为精确的标定，换用更高精度的惯性姿态测量设备重做实验，等等。

其次，算法暂时还没有做到完全实时化。本文方法的数据处理速度与RANSAC算法的迭代次数设置有关。对于开阔路面，一般选择20～30次迭代即可满足精度要求，此时对于一般配置的计算机（酷睿双核2.6 GHz CPU，内存2 GB），在MATLAB程序环境下的实时测量频率约为60 Hz～65 Hz，稍低于扫描仪75 Hz的数据采集频率。下一步计划将算法程序在C语言环境下编写或硬件化实现，可大幅提高处理速度，并满足一般场合下对测量带宽的需求。

最后，本文方法使用了两台二维激光扫描仪，这使得系统结构较为笨重，且成本也相对较高（但即使这样，相对于惯性测量系统仍然具有明显的价格优势）。我们正在研究只使用单台二维激光扫描仪实现同样测量效果的方案可行性，该工作现已取得初步进展，预期可使方法向实用化方向迈进一大步。

［1］ Nister D，Naroditsky O，Bergen J.Visual Odometry［C］//Proc Int Conf Computer Vision and Pattern Recognition.2004:652-659.

［2］ Friedrich Fraundorfer，Davide Scaramuzza.Visual Odometry Part I: TheFirst30YearsandFundamentals［J］.IEEERBOTICS&AutomationMagazine，2011，11:80-92.

［3］ 曹毓，冯莹，赵立双，等.相机姿态安装误差对单目视觉定位精度的影响［J］.传感器与微系统，2012，31（12）:23-26.

［4］ 王景川，陈卫东，胡仕煜，等.基于近红外视觉的机器人室外定位系统［J］.机器人，2010，32（1）:97-103.

［5］ 潘良晨，陈卫东.室内移动机器人的视觉定位方法研究［J］.机器人，2008，28（5）:504-509.

［6］ 张霄汉，陈小平，李嘉玲，等.一种基于视觉的步行机器人Monte Carlo自定位系统［J］.机器人，2006，28（4）:415-421

［7］ Feng Guohu，Wu Wenqi，Cao Juliang.Algorithm for Monocular Visual Odometry/SINS Integrated Navigation［J］.Journal of Chinese Inertial Technology，2011，19（3）:302-306.

［8］ Davide Scaramuzza，Siegwart R.Monocular Omnidirectional Visual Odometry for Outdoor Ground Vehicles［J］.ICVS，2008:206-215.

［9］ 陈艳，张漫，马文强，等.基于GPS和机器视觉的组合导航定位方法［J］.农业工程学报，2011，27（3）:126-130.

［10］曹毓，冯莹，杨云涛，等.RANSAC直线估计方法在路面三维点云优化中的应用［J］.红外与激光工程，2012，41（11）:3108-3112.

［11］冯莹，曹毓，雷兵，等.一种动态平台姿态和高度测量方法:中国，CN201310038521.1［P］.2013-01-31/2013-05-08.

［12］刘程熙，彭家寅.异面直线公垂线方程的求法［J］.内江师范学院学报，2009，24（8）:90-92.

曹毓（1982-），男，安徽五河人，在站博士后，工程师.研究方向为光学传感技术，计算成像技术，m15273136488@ 163.com。

Real-Time Measurement of Camera Attitude and Height in Visual Odometer*

CAO Yu1*，ZHANG Xiaohu1，FENG Ying2
（1.College of Aerospace Science and Engineering，National University of Defense Technology，Changsha 410073，China；2.College of Optoelectronic Science and Engineering，National University of Defense Technology，Changsha 410073，China）

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 precision，so we bring forward an real-time method to measure the camera's external parameters based on the laser scanning of road surface.Two 2D laser scanner orthogonal installed and downward scan，then the linear equation was estimated using the RANSAC algorithm.After that，road plane equation is obtained，which is the reference of the camera's attitude and altitude data.the indoor experiment results showed that:in the static conditions，the measurement error of the attitude is about 0.1 degree，maximum height measurement error is about 6 mm；the outdoor dynamic experimental results showed that:different from the traditional method of inertial measurement，camera external parameters measurement results are not affected by the vehicle deceleration influence，and the attitude measurement 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.

computer vision；visual odometer；laser scanning；attitude measurement；height measurement

TP393

A

1004-1699（2015）09-1354-07

项目来源:博士后基金项目（2014m562649）

2015-05-05修改日期:2015-07-09