第43卷第5期自动化学报Vol.43,No.5 2017年5月ACTA AUTOMATICA SINICA May,2017
俞毓锋1,2赵卉菁1,2崔锦实1,2査红彬1,2
摘要高精度定位是实现自动驾驶的关键.在城市密集区域，全球定位系统(Global positioning system,GPS)等卫星定位系统受到遮挡、干扰、多路径反射等影响，无法保障自动驾驶所需的定位精度.视觉定位技术通过图像特征匹配进行位置估计，被广泛研究.然而传统基于特征点的方法容易受到移动目标的干扰，在高动态交通场景中的应用面临挑战.在结构化道路场景中，车道等线特征普遍存在，为人类驾驶员的视觉理解与决策提供重要线索.受该思路的启发，本文利用场景中的三垂线和点特征构建道路结构特征(Road structural feature,RSF)，并在此基础上提出一个基于道路结构特征的单目视觉定位算法.本文利用在北京市区的典型路口、路段、街道等场所采集的车载视频数据进行实验验证，以同步采集的高精度GPS惯性导航组合定位系统数据为参照，与传统视觉定位算法进行比较.结果表明，本文算法在朝向估计上明显优于传统算法，对环境中的动态干扰有更高的鲁棒性.在卫星信号易受干扰的区域，可以有效地弥补GPS等定位系统的不足，为满足自动驾驶所需的车道级定位要求提供重要的技术手段.
关键词单目相机，视觉定位，道路结构特征，三垂线
引用格式俞毓锋，赵卉菁，崔锦实，査红彬.基于道路结构特征的智能车单目视觉定位.自动化学报,2017,43(5):725−734 DOI10.16383/j.aas.2017.c160413
Road Structural Feature Based Monocular Visual
Localization for Intelligent Vehicle
YU Yu-Feng1,2ZHAO Hui-Jing1,2CUI Jin-Shi1,2ZHA Hong-Bin1,2
Abstract Precise localization is an essential issue for autonomous driving applications,while global positioning system (GPS)-based systems are challenged to meet requirements such as lane-level accuracy,especially in crowded urban envi-ronment.This paper introduces a new visual-based localization approach in dynamic traffic environments,focusing on structured roads like straight roads or intersections.Such environments show several line segments on lane markings, curbs,poles,building edges,etc.,which demonstrate the road s longitude,latitude and vertical directions.Based on this observation,we define a road structural feature(RSF)as sets of segments along three perpendicular axes together with feature points,and propose an RSF based monocular visual localization method.Extensive experiments are conducted on three typical scenarios,including highway,intersection and downtown streets.Results show better accuracy compared with a state-of-the-
art visual localization method using feature points.We demonstrate that the proposed method can help improving localization accuracy in GPS restricted area,and discuss the remained challenges leading to future studies. Key words Monocular camera,visual localization,road structural feature(RSF),perpendicular axes
Citation Yu Yu-Feng,Zhao Hui-Jing,Cui Jin-Shi,Zha Hong-Bin.Road structural feature based monocular visual localization for intelligent vehicle.Acta Automatica Sinica,2017,43(5):725−734
智能车或自动驾驶技术受到广泛的关注，精确的定位技术是其重要环节.面向城市道路环境，车道级定位是实现智能车轨迹规划或决策的关键.全球定位系统(Global positioning system,GPS)被收稿日期2016-05-23录用日期2016-12-27
Manuscript received May23,2016;accepted December27,2016国家高技术研究发展计划(863计划)(2012AA011801)，国家自然科学基金(61573027)资助
Supported by National High Technology Research and Devel-opment Program of China(863Program)(2012AA011801),and National Natural Science Foundation of China(61573027)
本文责任编委王飞跃
Recommended by Associate Editor WANG Fei-Yue
1.北京大学信息科学技术学院北京100871
2.北京大学机器感知与智能教育部重点实验室北京100871
1.School of Electronic Engineering and Computer Science, Peking University,Beijing100871
2.Key Laboratory of Ma-chine Perception(Ministry of Education),Peking University, Beijing100871广泛运用于汽车导航系统.单点GPS定位误差为数米，且受到信号遮挡、干扰、多路径反射等影响，在城市密集区域，无法保障自动驾驶所需的定位精度.而以视觉里程计为代表，通过环境数据匹配进行运动估计的视觉定位方法可以有效提高相对定位精度.但城市环境中来自车辆、行人等动态目标的干扰给传统视觉定位算法带来了巨大挑战.如图1(a)所示，在一帧图像中，共检测了785个特征点，其中有277个在周围车辆上.移动干扰特征点的比率已经超出了视觉定位算法中外点滤除的极限，导致定位结果产生偏差.
在人类驾驶员视觉理解过程中，相比于特征点，车道等结构化道路信息起到更重要作用.以图1(b)为例，图像中包括沿道路方向、水平垂直道路方向和竖直方向的三种线特征，这些信息为驾驶员有效定
726自动化学报43卷
位自身车辆与道路的关系提供重要线索.受该思路启发，本文提出一个基于道路结构特征(Road struc-tural feature,RSF)的智能车单目视觉定位算法，用于解决高动态结构化道路环境下的定位问题.本文结合场景中的三垂线和点特征，定义道路结构特征，并在此基础上建立定位算法，包括道路结构预测、特征提取和车辆运动估计.本文利用安装在智能车平台上的摄像头在北京市区的路口、拥堵路段、密集街道等典型复杂场景进行数据采集，对本文提出的算法进行实验分析，以同步采集的高精度GPS惯性导航组合定位系统(GPS/惯导)数据为参照，与传统视觉定位算法进行比较，验证本文算法的有效性
.
图1单帧图像的点特征和线特征
Fig.1Point feature and Line feature of
one on-road image
本文结构如下：第1节围绕近年来国内外视觉定位的前沿研究，分析其在大范围高动态交通场景应用中所面临的关键难点；第2节给出道路结构特征的定义，并概述基于道路结构特征的定位算法；第3节详细描述定位算法中的细节；第4节给出实验结果，并与高精度GPS/惯导和传统视觉定位方法作比较；第5节总结本文算法并给出今后的研究展望.
1相关研究
基于视觉的定位算法被广泛用于智能车定位系统，视觉里程计(Visual odometry)[1]是其中最重要的领域
之一.视觉里程计利用单个或者多个相机的输入信息，通过前后帧特征匹配，估计本体的运动.有关视觉里程计的研究最早可以追溯到1980年.当时,Moravec等[2]利用一个可滑动相机获取视觉信息，并以此作为输入，完成了机器人的室内导航. 1987年,Matthies等[3]提出了利用视觉做定位的概念，设计了包含特征提取、特征匹配与跟踪、运动估计的理论框架.2004年,Nist´e r等[4]首次明确了视觉里程计这一概念，并在2006年详细描述了视觉里程计系统[5].他们设计了一种实时的视觉里程计系统，真正意义上实现了机器人室外运动导航，同时提出了两类视觉里程计的实现途径和流程，即单目视觉和立体视觉的方法，之后的研究大多在此基础上进行改进.
通用的视觉里程计以特征点匹配为基础，定位结果受到累计误差和移动物体干扰两方面因素的影响.针对累计误差，光束平差法(Bundle adjust-ment)[6]是一个常用的处理方法，通过最小化三维特征点在不同图像上的重投影误差，同时优化三维特征点和相机位置，提高了定位精度，但计算代价较高.为降低时间复杂度,Mouragnon等[7]使用局部光束平差法，在处理累计误差和计算代价上做了一个平衡.针对移动物体的干扰，传统的方法将移动物体上的特征点当作噪声，利用随机采样一致性方法(Random sample consensus,RANSAC)[8]来滤除噪声，但这只能处理少量干扰问题.Ozden 等[9]建立了一个多模型的定位与场景重建方法，将视频中的特征点进行聚类，提取移动物体，去除干扰.Kundu等[10]将光束平差法与多模型方法结合，有效提高了定位精度，但在移动物体较多的环境下，有效性受到制约.近几年的视觉里程计的研究将定位和建图结合起来，建立视觉SLAM方法，例如lsd-SLAM[11]、Orb-SLAM[12]等.此类方
法有效减少了累计误差，但由于需要在定位的同时重建三维场景，导致高动态环境下移动物体的干扰增加.
另一类视觉定位算法则采用与地图匹配的方式进行定位.Geiger等[13]利用视觉里程计生成的轨迹和二维地图上的道路进行形状比对，确定车辆实际位置.算法可定位到所处道路，但无法到达车道级精度.Ramalingam等[14−15]直接利用城市的三维模型，将图像中的特征点和线投影到三维模型中，直接获取对应的三维位置，避免了三维重建过程中的误差.算法提高了定位精度，但目前三维模型的可获取性不够广泛，算法的局限性高.另一些研究者则采用智能车预先建立地图进行定位.Achar等[16]利用词袋(Bag-of-words)的方法，在需要定位的街道上预先行驶若干圈采集数据，生成特征地图，在线定位时，将图像特征点和数据库中特征点进行匹配并定位.Ziegler等[17]同时建立了道路的几何地图与特征点云地图，提高了在线匹配的精度.该智能车完成了103千米的自动驾驶，但地图建立存在大量手工操作，人力和时间成本高.
传统的视觉定位算法使用特征点进行定位，在高动态交通场景中，特征点易受移动车辆的干扰，导致定位结果出现偏差.Geiger等[18]将图像特征投影到路面上，利用标志线的角点对定位进行优化，适
5期俞毓锋等：基于道路结构特征的智能车单目视觉定位727
用于低动态场景，但无法处理拥堵环境.考虑到道
路场景中线特征的朝向特殊性,Zhang 等[19]利用室外的垂直线特征进行定位，将原三维定位问题转换为二维定位问题，降低了复杂度，但垂直线特征不通用，局限性大.Barinova 等[20−21]则将线段进行聚类，并检测消失点，利用消失点之间的几何约束确定三垂线.该算法只在部分图片中进行测试，缺乏有效性验证，且计算代价高.本文对三垂线的检测进行优化，并将其用于结构化道路场景定位算法，极大地提高了定位估计的朝向稳定性.
2系统概述
本文提出的定位算法框架如图2所示.以下首先给出道路结构特征的定义，在此基础上简述算法的主要步骤，包含道路结构的预测、道路结构特征提取和基于道路结构特征的位置估计
.
图2算法框架Fig.2System outline
2.1道路结构特征定义
道路结构特征是一组带有语意信息，描述典型结构化道路场景特征的基本元素集合.在本研究中，道路结构特征由图像中对应于道路行进方向、水平垂直道路方向和竖直方向的三组线特征L 1,L 2,L 3，以及点特征P 构成，即Z RSF :{L 1,L 2,L 3,P }.其中线特征L 1,L 2,L 3在道路坐标系下相互垂直，在本文中简称三垂线.
2.2道路结构预测
随着定位服务(Location based services,LBS)技术的发展，地图作为数据基础被广泛建立，部分已开放使用(例如OpenStreetMap).这些地图中包含道路中心线、路口节点等信息，为预测局部道路结构提供依据.如图3所示，对于直道场景，根据智能车当前的定位信息和相机标定参数，可在地图中确定相机的可视区域，结合地图中道路中心线信息，建立局部道路坐标系.利用车辆位置以及相机参数等信息，将局部道路坐标系映射到图像中，预测对应于道
路行进方向、水平垂直道路方向和竖直方向的三组线特征L 1,L 2,L 3的方向.对于路口场景，根据地图中道路中心线，将相机可视区域分割为一系列小区域，每个小区域都根据道路中心线定义自身的道路局部坐标系，再映射到图像中，将图像分为相应的区域，并在每个区域中预测对应的三组线特征方向
.
图3
道路局部三垂线结构预测，包含直道和路口两种情况Fig.3Map-based RSF prediction,including
straightway and intersection
2.3道路结构特征提取
道路结构特征的提取以线段检测算法和特征点检测算法为基础，实时检测图像中的线段和特征点，构建道路结构特征.对于线特征，定义线和道路坐标系三方向的距离函数，以此将图像中的线段进行分类.通常情况下，路面上的车道线、道路边界、车辆侧面的线段等会被分成第一类L 1；路面上的停止线、车辆后侧的横向线段、路牌上的横向线段会被分成第二类L 2；路灯、路边建筑物、大型车辆等的竖直方向线段会被分为第三类L 3.对于点特征，直接根据通用特征点检测结果，生成特征点集合P .
2.4基于道路结构特征的位置估计
车辆的位置估计采用随机采样一致性方法(RANSAC).在检测到所有道路结构特征Z RSF 后,
基于随机采样构建样本集合{C i
RSF }.一个有效的采样C RSF 包含三条线段和两个点.其中三条线段中
728自动化学报43卷
两条相互平行，并且垂直于第三条.
C RSF ={l 1,l 2∈L u ,l 3∈L v ,u =v,p 1,p 2∈P }
(1)
汽车gps
图4所示为当u =1,v =3的一个样例.l 1
和l 2为沿道路方向上的线段,l 3为竖直方向上的线段,p 1和p 2为图像中任意两个特征点.在利用采样C RSF 进行位置姿态x 求解的过程中，使用其中的线特征求解旋转，再使用点特征估计平移，平移尺度的估计则使用车速信息累计求解.位置姿态求解结果
使用后验概率p (Z RSF |x i k )p (x i
k |x k −1)进行评价.其
中p (Z RSF |x i k )为观测模型,p (x i
k |x k −1)为车辆运动模型.通过一定次数的随机采样C RSF (本文实验中采样次数为200)，最优位置估计结果ˆx k 通过最大化后验概率求得.
ˆx k =arg max x i k ∼C i
RSF
{p (Z RSF |x i k )p (x i
k |x k −1)}
(2)
图4道路结构特征采样样例，包含三条线段和两个特征点
Fig.4Sample of RSF candidate,including three line
segments and two points
3算法细节
3.1坐标系定义
车体与相机的坐标系定义如图5所示.定义x v
={R v ,s v }为车体在世界坐标系下的位置姿态.其中,R v 为旋转矩阵,s v 为平移向量.车体坐标系定义为右手系准则，其中原点位于车辆后轴中心,x ,y ,z 的方向分别为车辆右侧，前方和上方.相机固定在车顶前侧上方，定义x c ={R c ,s c }为相机的在世界坐标系下的位置姿态.相机坐标系定义为右手系准侧，其中原点为相机中心,x ,y ,z 的方向分别为相机右侧、下方和前方.相机和车体的刚性固定，车体坐标系到图像映射P vc =K [R vc |s vc ]可通过标定得到.其中K 为相机内参数,R vc 和s vc 分别为外参数中的旋转矩阵和平移向量，即车体在相机坐标系下的旋转矩阵和平移向量
.
图5车体与相机的坐标系定义
Fig.5
Coordinate frames of the vehicle and the camera
3.2道路结构预测
定义e 1=(1,0,0)T ,e 2=(0,1,0)T ,e 3=(0,0,1)T 为x ,y ,z 方向的单位向量，车体在世界坐标系下的三个方向可表示为
d x d y d z =R v
e 1e 2e 3
(3)其中,d y =R v e 2为车体朝向.定义{R v,k ,s v,k }为
车体在k 时刻的位置姿态,{ˆR v,k ,ˆs v,k }为对应时刻
的预测结果.假设车体的运动在朝向上变化均匀，则车辆在k 时刻的旋转矩阵可以通过前一帧的旋转矩阵和前两帧之间的相对旋转求解
ˆR v,k =R rel R v,k −1= R v,k −1R T v,k −2 R v,k −1(4)平移的预测则将车辆在两帧间的位移进行求和.
在较短的时间内，位移方向近似为前后帧车辆朝向的角平分线的方向，即
ˆs v,k ≈s v,k −1+d y,k −1+ˆd
y,k 2 t k
t k −1
v t d t (5)
其中,d y,k −1为车体k −1时刻的朝向,ˆd
y,k 为车体k 时刻朝向预测结果,v 为车速.相机在k 时刻的位置姿态{R c ,s c }通过车体的位置姿态预测结果和标定参数进行求解.
ˆR
c,k =ˆR v,k R cv (6)ˆs c,k =ˆs v,k +ˆR
v,k s cv (7)
世界坐标系下到相机的投影矩阵的预测为
ˆP c =K [ˆR T c,k |−ˆR T c,k ˆs c,k ]
(8)
车体和相机的位置预测完成后，道路结构通过
相机的投影矩阵变换到图像坐标系下，如图3所示.对于直道环境，只有一条道路线被投影到图像中，生成一个局部道路坐标系；对于路口环境，多条道路线被投影到图像中，将图像分割成不同区域，每个区域包含一个局部道路坐标系；对于弯道环境，用多段线段拟合弯道，再将其投影到图像中，与路口环境类似.对于每一个区域，道路坐标系对应三个朝
5期俞毓锋等：基于道路结构特征的智能车单目视觉定位729向d j(j=1,2,3)，分别为道路行进方向、水平垂直
道路方向和竖直方向.虽然对于二维地图，道路朝向
只有一个自由度，但为了系统的统一，以及今后有三
维道路地图后算法的可扩展性，使用旋转矩阵R r=
[d1d2d3]来描述道路朝向.
3.3道路结构特征提取
道路结构特征RSF定义为{L1,L2,L3,P}，即
三垂线和特征点的集合.道路结构特征的提取中最
重要的部分为从图像基础特征(线段和点)中建立道
路结构特征.对于特征点，直接使用通用特征点检测
跟踪结果；对于三垂线特征，利用预测的道路结构,
将通用线段特征依照朝向进行分类，关键点在于定
义图像线段和道路坐标系三方向的距离函数.
图像上的任意线段l i转换到相机坐标系下，构
成一个过相机中心的平面πl i=[l T i K0]T，其法线方
向为
n i=
K T l i
K T l i
(9)
定义线段l i到道路方向R r e j的距离函数为线段对应平面法线n i和道路方向R r e j夹角余弦值的平方，即
d2(l i,R r e j)=
n T
i
R T
c
R r e j
2
=
l T
i
KR T
c
R r e j
K T l i
2
(10)
对于任意线段l i，计算它和局部坐标系下三方向的距离，将其归类为距离最近的方向，并将其朝向记录为d i.当最小距离大于给定阈值 时，该线段则被当作噪声滤除.
3.4车体位置姿态估计
给定一个道路结构特征的采样C RSF，如式(1)所示.其中l1,l2∈L u为图像上的两条线段，对应两条在道路坐标系下的平行线;l3∈L v为第三条线段，在道路坐标系下垂直于前两条线段.p1,p2∈P 为两个特征点.令ˆd1,ˆd2,ˆd3为三垂线特征l1,l2,l3的朝向估计结果,p1和p2为当前帧检测到的特征点,p 1和p 2为前一帧的跟踪结果.
3.4.1旋转矩阵求解
根据线特征和对应朝向的距离定义，有
l T
1
KRˆd1=0(11)
l T
2
KRˆd2=0(12)
l T
3
KRˆd3=0(13)从特征采样C RSF中已知l1和l2在道路坐标系下的朝向一致，即ˆd1=ˆd2，则l1和l2在图像上的交点对应于该朝向的消失点，即
v1=l1×l2=λKRˆd1(14)根据罗德里格斯矩阵，旋转矩阵可改写为
R=(I−S)−1(I+S)(15)
S=
0−c−b
c0−a
b a0
(16)
将式(15)代入式(14)中，可得
(I−S)K−1(l1×l2)=λ(I+S)ˆd1(17)结合式(16)和式(17)，共有4个参数a,b,c,λ需要求解.式(17)和式(13)共给出4个约束方程，通过联立方程组可求解这4个参数.在求解过程中，最多产生两组可行解，这两组可行解对应l3的方向相反.通过和预测的旋转矩阵ˆR c做比较，可滤除错误解，留下唯一可行解.
3.4.2平移向量求解
在世界坐标系下，令当前帧相机的投影矩阵为P=K[R|t]，前一帧相机的投影矩阵为P = K[R |t ]，则相机的相对旋转和平移为
R rel=RR T(18)
t rel=t−R T
rel
t (19)根据对极几何约束可得
p T F p =0(20)其中,
F=K−T[t rel]
×
R rel K−1(21)由于旋转矩阵已知，式(20)中待求解的只有平移向量t rel，共三个自由度.使用两个特征点，即可通过式(20)求解出归一化的平移向量，尺度因子 t rel 则需要通过其他方式求解.在本文实验中，尺度因子通过车轮编码器所获取的车速信息求解.相对位移求解完成后，通过式(19)求解当前相机的绝对位置，再利用标定参数，得到车体的绝对位置. 3.4.3后验概率估计
每一个采样C RSF都可求解出相应的旋转矩阵和平移向量.由于采样的随机性，需要多次采样来得到最优解.而最优解的估计可以通过最大化后验概率p(Z RSF|x i k)p(x i k|x k−1)得到.其中,p(x i k|x k−1)为运动模型，通过车辆的基础运动模型加上高斯噪声求解;p(Z RSF|x i k)为观测模型，需要定义观测值与真值的误差函数来求解.
本文误差函数定义为线特征与点特征的误差加权和，即
E=E l+λE p(22)
发布评论