CN106651953B - 一种基于交通指示牌的车辆位姿估计方法 - Google Patents

Abstract

本发明涉及一种基于交通指示牌的车辆位姿估计方法，首先通过常规GPS设备获得车辆概略位置，通过安装在车辆前方的单目相机获取车辆前方的影像。之后在一帧图像中检测交通指示牌，如未检测到指示牌即处理下一帧图像。检测得到交通指示牌后，提取指示牌的4个控制点坐标，再通过GPS概略信息获得数据库中预存的该指示牌的4个正交控制点坐标，4对控制点之间一一对应，计算由输入图像中指示牌位置到数据库正交位置的平面透视变换矩阵，由该矩阵解算得到车辆在交通指示牌坐标系下的精确位置和姿态。当车辆在设有交通指示牌的道路上行驶时，本发明能够快速定位车辆在指示牌坐标系下的位置和姿态，具有实时性高，成本低廉，易于大规模推广的优点。

Description

一种基于交通指示牌的车辆位姿估计方法

技术领域

本发明涉及一种基于交通指示牌的车辆位姿估计方法，属于数字图像处理技术和计算机视觉技术领域。

背景技术

目前，国际上很多国家和包括谷歌、百度在内的公司均在开展无人驾驶汽车研究，部分无人驾驶试验车已经进入实际道路测试阶段。美国内华达州机动车辆管理局已正式向谷歌颁发首张无人驾驶车辆牌照，预示着在不久的将来无人驾驶汽车将进入大众的生活，并改变人们的出行方式。

车辆自主定位作为汽车智能驾驶的基础，已成为该领域的关键技术。基于多传感器级联的定位技术相对较为成熟，能够满足高精度和实时性的要求，但部分传感器设备成本昂贵，其中常用的激光扫描系统价值便超过100万元/套，阻碍了系统的产品化，也无法在民用领域进行普及。同时，大多数依靠传感器级联的定位系统，在复杂的城市环境和拥堵的道路状况下，定位会出现累积误差并导致较大的定位误差。基于计算机视觉技术的定位方法能极大降低定位系统的成本，常用的视觉定位技术主要包括双目相机和单目相机，双目相机面临相机同步和立体匹配的难题，而基于单目相机的视觉定位技术大多依赖于复杂庞大的数据库系统，前期数据库构建工作繁重，但成本相对于多传感器级联的定位系统已经大大降低。

单目视觉定位技术大致可以分为三类：一是基于路面特征(包括车道线、路面导向标志和路面纹理)检测，二是基于3D城市模型和实际场景匹配，三是融合前两类的定位系统。第一和三类定位方法受限于地面纹理的清晰度，尤其是在车流量大且经常拥堵的城市交通路口，路面车道线和导向标志被密集车流遮挡，无法检测，因此在城市交通路口无法很好的完成车辆自定位；同时遇到雨、雪天气，路面纹理被覆盖时，该类方法也无法实现车辆定位。第二类需要构建的数据库及其复杂和庞大，且城市建筑物建设和更新速度较快，数据库后期维护的难度也较大。

目前国内基于计算机视觉的车辆定位技术研究开展较少，还处于研究起步阶段，如果可以提供一种成本低、精度高、简便易行的方法，能够有效解决城市复杂交通路口和拥堵路段的车辆位姿估计问题，使定位精度达到车道级，短期内与车载导航系统相联，可以准确判断车辆所行驶车道和车辆航向，长远看来可以使我国无人驾驶技术取得一定发展，在部分领域内赶超发达国家。

发明内容

针对现有技术的不足，本发明提出了一种基于交通指示牌的车辆位姿估计方法；

本发明可以实现快速准确获得行驶车辆的车道级定位信息，以及行驶车辆的实时姿态。

术语解释：

1、HSV颜色空间，是指以色调H、饱和度S和亮度V为参数的颜色模型；

2、SVM分类器，是特征空间上的间隔最大的线性分类器，通过正负样本训练得到，而后使用其对图像进行分类；

3、正样本，是指属于某一类别的样本；

4、负样本，是指不属于某一类别的样本；

5、Hough变换，是图像处理中从图像中识别几何形状的基本方法之一。

本发明的技术方案为：

一种基于交通指示牌的车辆位姿估计方法，所述交通指示牌是指位于道路两侧上方显著位置的平面矩形目标，其底色主要以蓝色、绿色或者黄色等为主；包括步骤如下：

A、构建数据库

所述数据库包括各个交通指示牌的以下信息：地理坐标、交通指示牌的尺寸大小、交通指示牌与道路夹角、交通指示牌处车道信息、底色，所述地理坐标是指交通指示牌所在的经度、纬度；所述交通指示牌的尺寸大小包括交通指示牌的长度值与宽度值；所述交通指示牌处车道信息包括道路上的车道数量、车道的宽度以及车道导向；所述地理坐标为索引；

B、车辆位姿估计

a、通过安装在车辆前方的单目相机实时获取道路图像，所述单目相机光轴的指向与车辆的行驶方向相同，并且光轴延伸方向与车辆所在道路平面法向量垂直；

b、将步骤a获取的道路图像由RGB颜色空间转换至HSV颜色空间；采用RGB阈值法检测交通指示牌受到光照、亮度等影响极大，为获得稳定的受光照、亮度等等影响较小的阈值检测方法，我们将获取的道路图像由RGB颜色空间转换至HSV颜色空间。

c、对步骤b获取的图像进行HSV阈值处理，符合阈值的区域像素设置为255，否则，设置为0，得到初步候选区域；

d、对步骤c获取的初步候选区域进行去噪处理，去除面积较小、宽高比例过大、宽高比例过小的噪声区域；

e、训练SVM分类器，从步骤d去噪处理后的剩余区域中得到目标区域：选取形状、位置、颜色先验知识作为特征，所述形状为宽高比例取值范围为1-2的矩形，所述位置为所述区域上部2/3的区域，所述颜色为蓝色或白色，采集大量正样本、负样本进行训练，从剩余区域中获得目标区域；

f、精确提取目标交通指示牌控制点坐标：运用轮廓提取和直线检测算法，获得目标交通指示牌四个顶点的坐标，即为目标交通指示牌控制点坐标；

g、解算车辆位姿参数。

根据本发明优选的，所述步骤b，包括步骤如下：

(1)通过常规GPS获得车辆的概略位置信息，即该车辆所在位置的经度、纬度，从数据库中查找距离该概略位置小于或等于D_t的关联交通指示牌的信息并提取，D_t的取值范围为50-100m；D_t的取值可以根据道路实际情况确定，直道可设置在100米左右，弯道可设置在50米左右。

(2)由步骤(1)中提取的关联交通指示牌的底色，确定色调H的阈值大小；例如，底色为蓝色时200＜H＜280，底色为绿色时120＜H＜200，底色为黄色时40＜H＜120；饱和度S的阈值取值范围为0.35＜S＜1，亮度V的阈值取值范围为0.35＜V＜1。饱和度和亮度由先验知识确定合理的阈值范围，在该饱和度和亮度阈值取值范围内检测正确率可以达到95％以上。

根据本发明优选的，所述步骤c，包括步骤如下：

(3)采用H、S、V的阈值取值范围对道路图像中的所有像素进行遍历，如果该像素均符合H、S、V的阈值取值范围，则该像素值设置为255，否则，该像素值设置为0，最终得到二值化图像I_b；

(4)选取一个大小为n×n的正方形窗，5＜n＜20，对二值化图像I_b进行形态学的闭操作处理，得到二值化图像I_b'；由于实际图像中，受背景及光照影响，得到的二值化图像I_b必然存在较多干扰点，经过上述处理便可以去除较小的干扰点，使之形成一个连通的整体；

根据本发明优选的，所述步骤d，包括步骤如下：

(5)对二值化图像I_b'中白色连通区域进行标号区分并计算面积，任意标号i代表一个白色连通区域整体，计算任意标号i代表的白色连通区域中白色像素点的数目A_i作为其面积；设定白色连通区域面积阈值最大值A_max及A_min，以1920×1080大小的二值化图像I_b'为例，A_max可设置为100000像素，A_min可设置为500像素。对于任意标号为i的白色连通区域，如果符合A_max＞A_i＞A_min，则该白色连通区域保留，否则，去除；由此得到更新后的二值化图像I_u；由先验知识可知，图像中面积过大或者过小的区域均不是最后的目标指示牌区域，经过面积约束条件后，可去除大部分干扰区域；

(6)对二值化图像I_u中剩余的区域分别计算宽高比，任意剩余区域j的横坐标最小值x_jmin和横坐标最大值x_jmax，以及纵坐标最小值y_jmin和纵坐标最大值y_jmax，任意剩余区域j的宽高比例P_j＝(x_jmax-x_jmin)/(y_jmax-y_jmin)；设定宽高比例阈值最大值P_max为2:1、宽高比例阈值最小值P_min为1:1，对于任意剩余区域j，如果符合P_max＞P_i＞P_min，则该剩余区域j保留，否则，去除，得到进一步更新的二值化图像I_u′；由先验知识可知，交通指示牌的宽高比例固定在一定的范围之内，实验表明，该宽高比例可以大大减少干扰区域对检测结果的影响。经过宽高比例约束条件后，可去除部分干扰区域。

根据本发明优选的，所述步骤e，包括步骤如下：

运用SVM分类器对二值化图像I_u′中剩余的区域进行分类，如果从二值化图像I_u′中得到两个以上的区域，则选取面积最大的一个区域作为目标区域R_t，如果从I_u′中得到唯一的区域，确定该区域为目标区域R_t。

根据本发明优选的，所述步骤f，包括步骤如下：

(7)在目标区域R_t中，调用OpenCV中的轮廓检测函数，通过轮廓面积排除法保留目标区域R_t最外围轮廓，方法同步骤(5)；

(8)使用OpenCV中HoughLines函数检测直线，通过检测得到的4条直线两两相交求得目标区域R_t的四个顶点坐标，即目标交通指示牌的4个控制点坐标P_In。如遇到四边形一个角被遮挡，即检测得到5条直线，则通过OpenCV中HoughLines函数改变斜率参数，去掉斜率不符合要求的直线，获得较为可靠的被遮挡顶点的坐标。获得的四个顶点坐标作为指示牌控制点坐标。

根据本发明优选的，所述步骤g，包括以下步骤：

(9)使用Matlab摄像机标定工具箱对单目相机进行标定，获得单目相机的内参数矩阵K，K为3×3矩阵，包含单目相机的焦距参数和图像中心参数；

(10)提取目标交通指示牌的4个控制点坐标P_In；

(11)通过常规GPS从数据库中获取关联交通指示牌的尺寸大小，该尺寸大小经由单目相机内参数矩阵K转换为关联交通指示牌的4个顶点的坐标P_d；

(12)目标交通指示牌的4个控制点坐标P_In与关联交通指示牌的4个顶点的坐标P_d一一对应，设定世界坐标系的原点为目标交通指示牌的中心，计算由目标交通指示牌位置到数据库中关联交通指示牌位置的平面透视变换矩阵M；

(13)关联交通指示牌的4个顶点确定的标准正交单应性矩阵H_db如式(Ⅰ)所示：

H_db＝K[r_db1 r_db2 r_db3 t_db] (Ⅰ)

式(Ⅰ)中，[r_db1 r_db2 r_db3 t_db]为数据库模拟拍摄标准正交图像时的单目相机外参数矩阵，r_db1,r_db2,r_db3为数据库模拟拍摄标准正交图像时的单目相机在世界坐标系中分别绕X轴、Y轴、Z轴旋转得到的旋转向量，t_db为数据库模拟拍摄标准正交图像时的单目相机相对世界坐标系原点沿X轴、Y轴、Z轴的平移向量；

关联交通指示牌的4个顶点均处于1个平面，世界坐标系中的Z轴坐标均为0，因此可省略r_db3，同时由正交关系可得式(II)：

式(II)中，d代表单目相机光心到目标交通指示牌的距离；

(14)目标交通指示牌的单应性矩阵H_In表示式(Ⅲ)所示：

H_In＝K[r_In1 r_In2 t_In] (Ⅲ)

式(Ⅲ)中，[r_In1 r_In2 t_In]为单目相机在道路上拍摄时的外参数矩阵，r_In1,r_In2为单目相机在道路上拍摄时围绕X轴、Y轴旋转得到的旋转矩阵，t_In为单目相机在道路上拍摄时沿世界坐标系X轴、Y轴、Z轴的平移向量；

图像像素坐标系与世界坐标系关系为p＝KRTP_W，p为像素坐标，P_W为世界坐标，RT为旋转与平移矩阵，求得目标交通指示牌的单应性矩阵H_In如式(Ⅳ)所示：

H_In＝MH_db (Ⅳ)

则得到式(Ⅴ)：

[r_In1 r_In2 t_In]＝K^-1MK[r_db1 r_db2 t_db] (Ⅴ)

(15)构造3×3旋转矩阵R＝[r_p1 r_p2 r_p3]，r_p1 r_p2 r_p3为单目相机分别绕世界坐标系X轴、Y轴、Z轴的旋转向量，由于旋转向量相互正交，求出r_p3＝r_p1×r_p2；此时的旋转矩阵R并不精确。对R进行奇异值分解，得到R＝UDV^T，U为3×3酉矩阵，D为3×3对角阵，V为3×3酉矩阵；

由于R本身是正交的，所以D＝I，I为单位矩阵，根据(Ⅵ)将R强制计算为精确的旋转矩阵R′：

R′＝UIV^T (Ⅵ)

利用罗德里格斯变换，将R变换为3×1的向量r_p＝[α β θ]，r_p即为单目相机旋转向量，得到相机的实时姿态，因相机光轴与车辆行驶方向重合，r_p表示车辆的实时走向和姿态，α代表车辆俯仰角，β代表车辆航向角，θ代表车辆横滚角；

通过式(Ⅶ)计算平移矩阵T_p：

T_p＝[T_p1 T_p2 T_p3]＝R′^-1[r_In1 r_In2 t_In] (Ⅶ)

式(Ⅶ)中，T_p为3×3矩阵，T_p1,T_p2和T_p3为矩阵T_p的三个列向量，R′^-1为经强制转换后旋转矩阵的逆；

利用式(Ⅷ)，对T_p3进行归一化处理，求出单目相机位置，即单目相机在以交通指示牌中心为原点的世界坐标系下的坐标t_p：

式(Ⅷ)中，Δx代表车载单目相机到交通指示牌中心的横向距离，结合数据库预存的车道信息，推算出车辆所在车道；Δy代表单目相机到交通指示牌中心的高差，由此推算出车辆所处位置的海拔；Δz代表单目相机到交通指示牌平面的法线距离，即车辆到交通指示牌的远近，至此得到车辆的6个位姿参数。

根据本发明优选的，通过手持式RTK-GPS仪测量得到交通指示牌的地理坐标。

本发明的有益效果有：

1、本发明通过将图像由RGB空间转换至HSV空间进行二值化处理能够有效避免光照强度变化对检测交通指示牌产生的影响。

2、本发明提出使用面积及宽高比例两个约束条件，确定交通指示牌候选区域，可以大大减少候选区域数量，缩短分类器判断时间。

3、本发明可以在夜间、凌晨、雨雪天气准确检测目标指示牌，是一种简单可靠的检测方法。

4、本发明通过平面透视变换矩阵解算得到车辆的位姿参数，无须计算复杂的基础矩阵，计算机量小，可靠性高，能够满足实时性要求。

5、本发明提出使用一种简易的数据库系统，该数据库结构简单、数据量小，并且易于后期维护，数据库内容主要包括各个交通指示牌的位置信息、尺寸大小、偏转角度、底色以及指示牌处道路的车道信息。

6、本发明仅需要普通单目相机及一个常规GPS设备，成本低廉，易于实现大规模推广。

附图说明：

图1为本发明基于交通指示牌车辆位姿估计的流程图；

图2为日间交通指示牌检测效果示意图；

图3为夜间交通指示牌检测效果示意图；

图4为凌晨交通指示牌检测效果示意图；

图5为部分遮挡情况下交通指示牌检测效果示意图；

图6为检测得到的控制点示意图。

具体实施方式

下面结合说明书附图和实施例对本发明作进一步限定，但不限于此。

实施例

一种基于交通指示牌的车辆位姿估计方法，如图1所示，交通指示牌是指位于道路两侧上方显著位置的平面矩形目标，其底色主要以蓝色、绿色或者黄色等为主；包括步骤如下：

A、构建数据库

数据库包括各个交通指示牌的以下信息：地理坐标、交通指示牌的尺寸大小、交通指示牌与道路夹角、交通指示牌处车道信息、底色，地理坐标是指交通指示牌所在的经度、纬度，通过手持式RTK-GPS仪测量得到交通指示牌的地理坐标；交通指示牌的尺寸大小包括交通指示牌的长度值与宽度值；交通指示牌处车道信息包括道路上的车道数量、车道的宽度以及车道导向；地理坐标为索引；

B、车辆位姿估计

a、通过安装在车辆前方的单目相机实时获取道路图像，单目相机光轴的指向与车辆的行驶方向相同，并且光轴延伸方向与车辆所在道路平面法向量垂直；

b、将步骤a获取的道路图像由RGB颜色空间转换至HSV颜色空间；采用RGB阈值法检测交通指示牌受到光照、亮度等影响极大，为获得稳定的受光照、亮度等等影响较小的阈值检测方法，我们将获取的道路图像由RGB颜色空间转换至HSV颜色空间。包括步骤如下：

(1)通过常规GPS获得车辆的概略位置信息，即该车辆所在位置的经度、纬度，从数据库中查找距离该概略位置小于或等于D_t的关联交通指示牌的信息并提取，D_t的取值范围为50-100m；D_t的取值可以根据道路实际情况确定，直道可设置在100米左右，弯道可设置在50米左右。

(2)由步骤(1)中提取的关联交通指示牌的底色，确定色调H的阈值大小；底色为蓝色时200＜H＜280，底色为绿色时，120＜H＜200，底色为黄色时，40＜H＜120；饱和度S的阈值取值范围为0.35＜S＜1，亮度V的阈值取值范围为0.35＜V＜1。饱和度和亮度由先验知识确定合理的阈值范围，在该饱和度和亮度阈值取值范围内检测正确率可以达到95％以上。

c、对步骤b获取的图像进行HSV阈值处理，符合阈值的区域像素设置为255，否则，设置为0，得到初步候选区域；包括步骤如下：

(3)采用H、S、V的阈值取值范围对道路图像中的所有像素进行遍历，如果该像素均符合H、S、V的阈值取值范围，则该像素值设置为255，否则，该像素值设置为0，最终得到二值化图像I_b；

(4)选取一个大小为n×n的正方形窗，5＜n＜20，对二值化图像I_b进行形态学的闭操作处理，得到二值化图像I_b'；由于实际图像中，受背景及光照影响，得到的二值化图像I_b必然存在较多干扰点，经过上述处理便可以去除较小的干扰点，使之形成一个连通的整体。

d、对步骤c获取的初步候选区域进行去噪处理，去除面积较小、宽高比例过大、宽高比例过小的噪声区域；包括步骤如下：

(5)对二值化图像I_b'中白色连通区域进行标号区分并计算面积，任意标号i代表一个白色连通区域整体，计算任意标号i代表的白色连通区域中白色像素点的数目A_i作为其面积；设定白色连通区域面积阈值最大值A_max及A_min，以1920×1080大小的二值化图像I_b'为例，A_max可设置为100000像素，A_min可设置为500像素。对于任意标号为i的白色连通区域，如果符合A_max＞A_i＞A_min，则该白色连通区域保留，否则，去除；由此得到更新后的二值化图像I_u；由先验知识可知，图像中面积过大或者过小的区域均不是最后的目标指示牌区域，经过面积约束条件后，可去除大部分干扰区域；

(6)对二值化图像I_u中剩余的区域分别计算宽高比，任意剩余区域j的横坐标最小值x_jmin和横坐标最大值x_jmax，以及纵坐标最小值y_jmin和纵坐标最大值y_jmax，任意剩余区域j的宽高比例P_j＝(x_jmax-x_jmin)/(y_jmax-y_jmin)；设定宽高比例阈值最大值P_max为2:1、宽高比例阈值最小值P_min为1:1，对于任意剩余区域j，如果符合P_max＞P_i＞P_min，则该剩余区域j保留，否则，去除，得到进一步更新的二值化图像I_u′；由先验知识可知，交通指示牌的宽高比例固定在一定的范围之内，实验表明，该宽高比例可以大大减少干扰区域对检测结果的影响。经过宽高比例约束条件后，可去除部分干扰区域。

e、训练SVM分类器，从步骤d去噪处理后的剩余区域中得到目标区域：选取形状、位置、颜色先验知识作为特征，所述形状为宽高比例取值范围为1-2的矩形，所述位置为所述区域上部2/3的区域，所述颜色为蓝色或白色，采集大量正样本、负样本进行训练，从剩余区域中获得目标区域；包括步骤如下：

运用SVM分类器对二值化图像I_u′中剩余的区域进行分类，如果从二值化图像I_u′中得到两个以上的区域，则选取面积最大的一个区域作为目标区域R_t，如果从I_u′中得到唯一的区域，确定该区域为目标区域R_t。

f、精确提取目标交通指示牌控制点坐标：运用轮廓提取和直线检测算法，获得目标交通指示牌四个顶点的坐标，即为目标交通指示牌控制点坐标；包括步骤如下：

(7)在目标区域R_t中，调用OpenCV中的轮廓检测函数，通过轮廓面积排除法保留目标区域R_t最外围轮廓，方法同步骤(5)；

(8)使用OpenCV中HoughLines函数检测直线，通过检测得到的4条直线两两相交求得目标区域R_t的四个顶点坐标，即目标交通指示牌的4个控制点坐标P_In。如遇到四边形一个角被遮挡，即检测得到5条直线，则通过OpenCV中HoughLines函数改变斜率参数，去掉斜率不符合要求的直线，获得较为可靠的被遮挡顶点的坐标。获得的四个顶点坐标作为指示牌控制点坐标。

g、解算车辆位姿参数。包括以下步骤：

(9)使用Matlab摄像机标定工具箱对单目相机进行标定，获得单目相机的内参数矩阵K，K为3×3矩阵，包含单目相机的焦距参数和图像中心参数；

(10)提取目标交通指示牌的4个控制点坐标P_In；

(11)通过常规GPS从数据库中获取关联交通指示牌的尺寸大小，该尺寸大小经由单目相机内参数矩阵K转换为关联交通指示牌的4个顶点的坐标P_d；

(12)目标交通指示牌的4个控制点坐标P_In与关联交通指示牌的4个顶点的坐标P_d一一对应，设定世界坐标系的原点为目标交通指示牌的中心，计算由目标交通指示牌位置到数据库中关联交通指示牌位置的平面透视变换矩阵M；

(13)关联交通指示牌的4个顶点确定的标准正交单应性矩阵H_db如式(Ⅰ)所示：

H_db＝K[r_db1 r_db2 r_db3 t_db] (Ⅰ)

式(Ⅰ)中，[r_db1 r_db2 r_db3 t_db]为数据库模拟拍摄标准正交图像时的单目相机外参数矩阵，r_db1,r_db2,r_db3为数据库模拟拍摄标准正交图像时的单目相机在世界坐标系中分别绕X轴、Y轴、Z轴旋转得到的旋转向量，t_db为数据库模拟拍摄标准正交图像时的单目相机相对世界坐标系原点沿X轴、Y轴、Z轴的平移向量；

关联交通指示牌的4个顶点均处于1个平面，世界坐标系中的Z轴坐标均为0，因此可省略r_db3，同时由正交关系可得式(II)：

式(II)中，d代表单目相机光心到目标交通指示牌的距离；

(14)目标交通指示牌的单应性矩阵H_In表示式(Ⅲ)所示：

H_In＝K[r_In1 r_In2 t_In] (Ⅲ)

式(Ⅲ)中，[r_In1 r_In2 t_In]为单目相机在道路上拍摄时的外参数矩阵，r_In1,r_In2为单目相机在道路上拍摄时围绕X轴、Y轴旋转得到的旋转矩阵，t_In为单目相机在道路上拍摄时沿世界坐标系X轴、Y轴、Z轴的平移向量；

图像像素坐标系与世界坐标系关系为p＝KRTP_W，p为像素坐标，P_W为世界坐标，RT为旋转与平移矩阵，求得目标交通指示牌的单应性矩阵H_In如式(Ⅳ)所示：

H_In＝MH_db (Ⅳ)

则得到式(Ⅴ)：

[r_In1 r_In2 t_In]＝K^-1MK[r_db1 r_db2 t_db] (Ⅴ)

(15)构造3×3旋转矩阵R＝[r_p1 r_p2 r_p3]，r_p1 r_p2 r_p3为单目相机分别绕世界坐标系X轴、Y轴、Z轴的旋转向量，由于旋转向量相互正交，求出r_p3＝r_p1×r_p2；此时的旋转矩阵R并不精确。对R进行奇异值分解，得到R＝UDV^T，U为3×3酉矩阵，D为3×3对角阵，V为3×3酉矩阵；

由于R本身是正交的，所以D＝I，I为单位矩阵，根据(Ⅵ)将R强制计算为精确的旋转矩阵R′：

R′＝UIV^T (Ⅵ)

利用罗德里格斯变换，将R变换为3×1的向量r_p＝[α β θ]，r_p即为单目相机旋转向量，得到相机的实时姿态，因相机光轴与车辆行驶方向重合，r_p表示车辆的实时走向和姿态，α代表车辆俯仰角，β代表车辆航向角，θ代表车辆横滚角；

通过式(Ⅶ)计算平移矩阵T_p：

T_p＝[T_p1 T_p2 T_p3]＝R′^-1[r_In1 r_In2 t_In] (Ⅶ)

式(Ⅶ)中，T_p为3×3矩阵，T_p1,T_p2和T_p3为矩阵T_p的三个列向量，R′^-1为经强制转换后旋转矩阵的逆；

利用式(Ⅷ)，对T_p3进行归一化处理，求出单目相机位置，即单目相机在以交通指示牌中心为原点的世界坐标系下的坐标t_p：

式(Ⅷ)中，Δx代表车载单目相机到交通指示牌中心的横向距离，结合数据库预存的车道信息，推算出车辆所在车道；Δy代表单目相机到交通指示牌中心的高差，由此推算出车辆所处位置的海拔；Δz代表单目相机到交通指示牌平面的法线距离，即车辆到交通指示牌的远近，至此得到车辆的6个位姿参数。

图2为根据本实施例所述方法求取的日间交通指示牌检测效果示意图；

图3为根据本实施例所述方法求取的夜间交通指示牌检测效果示意图；

图4为根据本实施例所述方法求取的凌晨交通指示牌检测效果示意图；

图5为根据本实施例所述方法求取的部分遮挡情况下交通指示牌检测效果示意图；

图2-图5中，白色粗线条将检测的交通指示牌标注；

图6为检测得到的交通指示牌的四个控制点示意图，由白色圆点标注。

由图2-图6可知，本发明所述方法可以在夜间、凌晨、雨雪天气准确检测目标交通指示牌，简单可靠。

Claims (2)

1.一种基于交通指示牌的车辆位姿估计方法，所述交通指示牌是指位于道路两侧上方显著位置的平面矩形目标，其特征在于，包括步骤如下：

A、构建数据库

所述数据库包括各个交通指示牌的以下信息：地理坐标、交通指示牌的尺寸大小、交通指示牌与道路夹角、交通指示牌处车道信息、底色，所述地理坐标是指交通指示牌所在的经度、纬度；所述交通指示牌的尺寸大小包括交通指示牌的长度值与宽度值；所述交通指示牌处车道信息包括道路上的车道数量、车道的宽度以及车道导向；所述地理坐标为索引；

B、车辆位姿估计

a、通过安装在车辆前方的单目相机实时获取道路图像，所述单目相机光轴的指向与车辆的行驶方向相同，并且光轴延伸方向与车辆所在道路平面法向量垂直；

b、将步骤a获取的道路图像由RGB颜色空间转换至HSV颜色空间；

c、对步骤b获取的图像进行HSV阈值处理，符合阈值的区域像素设置为255，否则，设置为0，得到初步候选区域；

d、对步骤c获取的初步候选区域进行去噪处理，去除面积较小、宽高比例过大、宽高比例过小的噪声区域；

e、训练SVM分类器，从步骤c去噪处理后的剩余区域中得到目标区域：选取形状、位置、颜色先验知识作为特征，所述形状为宽高比例取值范围为1-2的矩形，所述位置为所述区域上部2/3的区域，所述颜色为蓝色或白色，采集大量正样本、负样本进行训练，从剩余区域中获得目标区域；

f、精确提取目标交通指示牌控制点坐标：运用轮廓提取和直线检测算法，获得目标交通指示牌四个顶点的坐标，即为目标交通指示牌控制点坐标；

g、解算车辆位姿参数；

所述步骤b，包括步骤如下：

(1)通过常规GPS获得车辆的概略位置信息，即该车辆所在位置的经度、纬度，从数据库中查找距离该概略位置小于或等于D_t的关联交通指示牌的信息并提取，D_t的取值范围为50-100m；D_t

(2)由步骤(1)中提取的关联交通指示牌的底色，确定色调H的阈值大小；饱和度S的阈值取值范围为0.35<S<1，亮度V的阈值取值范围为0.35<V<1；

所述步骤c，包括步骤如下：

(3)采用H、S、V的阈值取值范围对道路图像中的所有像素进行遍历，如果该像素均符合H、S、V的阈值取值范围，则该像素值设置为255，否则，该像素值设置为0，最终得到二值化图像I_b；

(4)选取一个大小为n×n的正方形窗，5<n<20，对二值化图像I_b进行形态学的闭操作处理，得到二值化图像I_b'；

所述步骤d，包括步骤如下：

(5)对二值化图像I_b'中白色连通区域进行标号区分并计算面积，任意标号i代表一个白色连通区域整体，计算任意标号i代表的白色连通区域中白色像素点的数目A_i作为其面积；设定白色连通区域面积阈值最大值A_max及A_min，对于任意标号为i的白色连通区域，如果符合A_max>A_i>A_min，则该白色连通区域保留，否则，去除；由此得到更新后的二值化图像I_u；

(6)对二值化图像I_u中剩余的区域分别计算宽高比，任意剩余区域j的横坐标最小值x_jmin和横坐标最大值x_jmax，以及纵坐标最小值y_jmin和纵坐标最大值y_jmax，任意剩余区域j的宽高比例P_j＝(x_jmax-x_jmin)/(y_jmax-y_jmin)；设定宽高比例阈值最大值P_max为1:1、宽高比例阈值最小值P_min为1:2，对于任意剩余区域j，如果符合P_max>P_i>P_min，则该剩余区域j保留，否则，去除，得到进一步更新的二值化图像I_u′；

所述步骤e，包括步骤如下：

运用SVM分类器对二值化图像I_u′中剩余的区域进行分类，如果从二值化图像I_u′中得到两个以上的区域，则选取面积最大的一个区域作为目标区域R_t，如果从I_u′中得到唯一的区域，确定该区域为目标区域R_t；

所述步骤f，包括步骤如下：

(7)在目标区域R_t中，调用OpenCV中的轮廓检测函数，通过轮廓面积排除法保留目标区域R_t最外围轮廓；

(8)使用OpenCV中HoughLines函数检测直线，通过检测得到的4条直线两两相交求得目标区域R_t的四个顶点坐标，即目标交通指示牌的4个控制点坐标P_In；

所述步骤g，包括以下步骤：

(9)使用Matlab摄像机标定工具箱对单目相机进行标定，获得单目相机的内参数矩阵K，K为3×3矩阵，包含单目相机的焦距参数和图像中心参数；

(10)提取目标交通指示牌的4个控制点坐标P_In；

(11)通过常规GPS从数据库中获取关联交通指示牌的尺寸大小，该尺寸大小经由单目相机内参数矩阵K转换为关联交通指示牌的4个顶点的坐标P_d；

(12)目标交通指示牌的4个控制点坐标P_In与关联交通指示牌的4个顶点的坐标P_d一一对应，设定世界坐标系的原点为目标交通指示牌的中心，计算由目标交通指示牌位置到数据库中关联交通指示牌位置的平面透视变换矩阵M；

(13)关联交通指示牌的4个顶点确定的标准正交单应性矩阵H_db如式(Ⅰ)所示：

H_db＝K[r_db1 r_db2 r_db3 t_db] (Ⅰ)

式(Ⅰ)中，[r_db1 r_db2 r_db3 t_db]为数据库模拟拍摄标准正交图像时的单目相机外参数矩阵，r_db1,r_db2,r_db3为数据库模拟拍摄标准正交图像时的单目相机在世界坐标系中分别绕X轴、Y轴、Z轴旋转得到的旋转向量，t_db为数据库模拟拍摄标准正交图像时的单目相机相对世界坐标系原点沿X轴、Y轴、Z轴的平移向量；

关联交通指示牌的4个顶点均处于1个平面，世界坐标系中的Z轴坐标均为0，因此可省略r_db3，同时由正交关系可得式(Ⅱ)：

式(Ⅱ)中，d代表单目相机光心到目标交通指示牌的距离；

(14)目标交通指示牌的单应性矩阵H_In表示式(Ⅲ)所示：

H_In＝K[r_In1 r_In2 t_In] (Ⅲ)

式(Ⅲ)中，[r_In1 r_In2 t_In]为单目相机在道路上拍摄时的外参数矩阵，r_In1,r_In2为单目相机在道路上拍摄时围绕X轴、Y轴旋转得到的旋转矩阵，t_In为单目相机在道路上拍摄时沿世界坐标系X轴、Y轴、Z轴的平移向量；

图像像素坐标系与世界坐标系关系为p＝KRTP_W，p为像素坐标，P_W为世界坐标，RT为旋转与平移矩阵，求得目标交通指示牌的单应性矩阵H_In如式(Ⅳ)所示：

H_In＝MH_db (Ⅳ)

则得到式(Ⅴ)：

[r_In1 r_In2 t_In]＝K^-1MK[r_db1 r_db2 t_db] (Ⅴ)

(15)构造3×3旋转矩阵R＝[r_p1 r_p2 r_p3]，r_p1 r_p2 r_p3为单目相机分别绕世界坐标系X轴、Y轴、Z轴的旋转向量，由于旋转向量相互正交，求出r_p3＝r_p1×r_p2；对R进行奇异值分解，得到R＝UDV^T，U为3×3酉矩阵，D为3×3对角阵，V为3×3酉矩阵；

由于R本身是正交的，所以D＝I，I为单位矩阵，根据(Ⅵ)将R强制计算为精确的旋转矩阵R′：

R′＝UIV^T (Ⅵ)

利用罗德里格斯变换，将R变换为3×1的向量r_p＝[α β θ]，r_p即为单目相机旋转向量，得到相机的实时姿态，因相机光轴与车辆行驶方向重合，r_p表示车辆的实时走向和姿态，α代表车辆俯仰角，β代表车辆航向角，θ代表车辆横滚角；

通过式(VII)计算平移矩阵T_p：

T_p＝[T_p1 T_p2 T_p3]＝R′^-1[r_In1 r_In2 t_In] (VII)

式(VII)中，T_p为3×3矩阵，T_p1,T_p2和T_p3为矩阵T_p的三个列向量，R′^-1为经强制转换后旋转矩阵的逆；

利用式(Ⅷ)，对T_p3进行归一化处理，求出单目相机位置，即单目相机在以交通指示牌中心为原点的世界坐标系下的坐标t_p：

式(Ⅷ)中，△x代表车载单目相机到交通指示牌中心的横向距离，结合数据库预存的车道信息，推算出车辆所在车道；△y代表单目相机到交通指示牌中心的高差，由此推算出车辆所处位置的海拔；△z代表单目相机到交通指示牌平面的法线距离，即车辆到交通指示牌的远近，至此得到车辆的6个位姿参数。

2.根据权利要求1所述的一种基于交通指示牌的车辆位姿估计方法，其特征在于，通过手持式RTK-GPS仪测量得到交通指示牌的地理坐标。