[发明专利]一种基于交通指示牌的车辆位姿估计方法

权利要求书

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仪测量得到交通指示牌的地理坐标。