CN111065043A - 一种基于车路通信的隧道内车辆融合定位系统及方法 - Google Patents

Abstract

本发明请求保护一种基于车路通信的隧道内车辆融合定位系统及方法，系统由路侧单元与车载定位设备组成。路侧单元包括离线图像地图、图像匹配和路侧通信模块，车载定位设备包括图像预处理、车载通信、惯性导航和融合定位模块。预先在隧道中等间距地布置在该隧道内具有唯一性的图像标签，并采集含有标签的隧道图像，构造隧道的离线图像地图。车载定位设备利用图像传感器采集包含图像标签的车辆前方图像，进行预处理，并发送给路侧单元。路侧单元根据接收的图像与存储的隧道离线图像地图进行图像匹配定位，并将结果发送给车载定位设备。车载定位设备接收图像匹配定位结果，与惯性导航模块计算得到的位置数据进行融合，得到车辆在隧道内的准确定位。

Description

一种基于车路通信的隧道内车辆融合定位系统及方法

技术领域

本发明属于计算机、通信与自动化技术领域，具体涉及一种隧道内车辆融合定位系统及方法。

背景技术

在智能车辆自动驾驶领域，定位技术是最基础、关键的技术之一。目前，常用于对行驶车辆进行定位的方法为GPS(Global Position System,全球卫星定位系统)卫星定位方法，但当车辆行驶在隧道时，GPS卫星定位系统在隧道内无法接收卫星信号，进而无法对行驶车辆进行正常定位。

中国专利申请：一种导航系统及其导航方法(申请号：CN201010539918.5)公开了一种应用于隧道中的车载导航方法，该方法根据采集的GPS信号来判断车辆是否进入隧道，当车辆进入隧道之后，通过加速度模块获取车辆的加速度，然后通过积分运算算出车辆的行驶距离，最终根据车辆的行驶距离和隧道起始位置来判断车辆在隧道中的位置。但该方法只使用了加速度模块一种传感器，由于该传感器存在漂移，累计误差会不断增大。

中国专利申请:隧道定位方法及装置(申请号：CN201510896068.7)公开了一种隧道定位装置及方法，该方法根据隧道环境测量值以及目标隧道环境测量值与目标隧道位置信息之间的对应关系进行定位。所述的环境参数包括亮度测量值和气压测量值。该方法仅仅是通过隧道亮度和气压进行定位，这种方法只能进行粗略定位且存在较大误差。

中国专利申请：一种基于超宽带的车辆隧道定位系统(申请号：CN201621482140.8)公开了一种基于超宽带脉冲信号的车辆隧道定位系统，该系统根据多个超宽带脉冲信号到达车载超宽带定位模块的时间并结合脉冲信号中携带的信号发送时间来确定多个超宽带标签与车载超宽带定位模块的距离，然后根据超宽带脉冲信号中携带的超宽带标签的位置确定车载超宽带定位模块的位置。该系统虽然能够实现在隧道中对车辆进行定位，但是误差较大且需要额外增加设备。

中国专利申请：一种基于模拟GNSS信号隧道定位系统(申请号：CN201721033346.7)公开了一种模拟GNSS信号隧道定位系统，该系统通过获取的星历数据和时间数据以及设定好的位置经纬度数据计算得出GNSS信号，通过使用这种模拟信号弥补了车辆在隧道中获取不到真实GNSS信号的缺陷。但该系统只是弥补了隧道中的GNSS信号空白，并没有对GNSS信号的定位精度进行改善。

中国专利申请：基于北斗和RFID技术的车辆隧道定位系统(申请号：CN201620465267.2)公开了基于北斗和RFID技术的车辆隧道定位系统，该系统在车辆未进入隧道时使用北斗卫星数据对车辆进行定位，当车辆进入隧道时通过RFID标签的信息确定车辆的位置。该系统能够在缺少卫星数据的隧道内对车辆进行定位，但是定位精度会受到RFID标签间距的影响，定位精度不够高。

本发明针对现有车辆在隧道中不能进行精确有效定位的问题，提出一种隧道内车辆融合定位系统及方法。在该系统中，通过将实时采集的图像与离线图像地图中的图像进行特征匹配从而计算图像采集点与离线图像地图数据点之间的相对位置，然后再根据图像采集点与离线图像地图数据点之间的相对位置和离线图像地图数据点的实际位置计算出图像采集点的位置，最后通过融合定位模块将图像匹配定位模块得出的位置信息和惯性导航模块得到的位置信息进行融合，从而获得更加精确有效的定位结果。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种基于车路通信的隧道内车辆融合定位系统及方法。本发明的技术方案如下：

一种基于车路通信的隧道内车辆融合定位系统，包括路侧单元与车载定位设备，其中路侧单元包括离线图像地图模块、图像匹配模块和通信模块。离线图像地图模块用于存储隧道内的离线图像地图；通信模块用于接收车载定位设备预处理过的图像和发送图像匹配定位结果；图像匹配模块将接收到的图像与离线图像地图中的图像进行特征匹配从而计算图像采集点与离线图像地图数据点之间的相对位置，然后根据两者的相对位置和离线图像地图数据点的实际位置计算出图像采集点的位置。车载定位设备包括图像预处理模块、通信模块、惯性导航模块和融合定位模块。图像预处理模块将实时采集的图像进行畸变矫正和图像增强处理；通信模块用于车载定位设备与路侧单元之间的通信；惯性导航模块根据惯性测量单元的测量数据计算车辆当前位置信息；融合定位模块将惯性导航模块计算出的位置信息和图像匹配定位模块计算出的位置信息进行融合，从而获得更加精确的定位结果。

所述隧道需要每隔一段距离L设置一副大小为M*N图像标签，所述图像标签在该隧道内具有唯一性。

本专利所述位置坐标均为地球坐标系下的坐标。

进一步的，图像匹配定位模块将实时采集的图像与离线图像地图中的图像进行特征匹配从而计算图像采集点与离线图像地图数据点之间的相对位置，然后再根据图像采集点与离线图像地图数据点之间的相对位置和离线图像地图数据点的实际位置计算出图像采集点的位置。其步骤包括：

(1)图像采集及图像预处理。车载定位设备通过车载双目相机采集车辆前方图像并进行畸变矫正和图像增强处理；

(2)路测设备对采集的图像进行特征提取。以图像的ORB(Oriented FAST andRotated BRIEF)特征为例说明本专利方法流程，但不限于该特征；

(3)双目图像特征匹配。将双目相机采集的左右两幅图像特征匹配，以特征点间的汉明距离小于最小汉明距离的两倍为依据对特征点进行筛选；

(4)根据双目图像视差计算特征点深度。根据步骤(3)中的匹配结果和特征点的像素坐标，计算特征点深度，得到像素点的空间坐标；

(5)获取离线图像地图中的图像并提取特征。根据惯性导航系统提供的位置信息确定离线图像地图中的数据点q_i。选取数据点q_i及其前后数据点q_i-1和q_i+1中的图像并提取这三张图像ORB特征；

(6)实时图像与离线图像地图中的图像进行特征匹配。将采集的图像和步骤(5)中的三张图像进行特征匹配，以特征点间的汉明距离小于最小汉明距离的两倍为依据对特征点进行筛选。选择特征匹配点数最多的两幅图像计算图像采集点的位置；

(7)求解旋转矩阵R和平移向量t。根据步骤(6)中图像匹配结果利用PnP的原理求解图像采集点与离线图像地图数据点之间的旋转矩阵R和平移向量t；

(8)计算车辆位置。根据步骤(7)可以算出图像采集点相对于离线图像地图数据点之间的旋转矩阵R和平移向量t。离线图像地图数据点中存有该数据点的实际位置。由此，可以由旋转矩阵R、平移向量t和离线图像地图数据点的实际位置推算出车辆当前的位置。该位置信息以经纬度的方式表示。

进一步的，路侧单元用于完成图像匹配定位。车载定位设备在靠近隧道时与路侧单元进行通信连接。车载定位设备将采集的图像进行预处理后发送到路侧单元，然后由路侧单元完成实时图像与离线图像地图中图像的匹配定位工作。最终将匹配定位结果通过通信模块发送给车载定位设备。

本专利所述系统在实施之前需要构建各个隧道的离线图像地图。离线图像地图的构建步骤包括：

(1)在隧道中等间距地布置大小为M*N图像标签，所述图像标签在该隧道内具有唯一性；

(2)在隧道内设置离线图像地图数据点。各个数据点间的距离可以根据定位精度要求进行调整；

(3)在各个数据点处采集图像；

(4)保存各个数据点处的图像和位置信息。收集完隧道内所有数据点的图像和位置信息即完成了该隧道内离线图像地图的构建；

(5)将构建的离线图像地图存储到路侧单元中。

本发明的优点及有益效果如下：

本发明针对现有车辆在隧道中不能进行精确有效定位的问题，提出一种基于车路通信的隧道内车辆融合定位系统及方法来实现车辆在隧道中的精确定位。通过与路侧单元进行通信，使图像匹配定位的过程在路侧单元中进行，从而减少了车载定位设备计算量。图像匹配定位结果与惯性导航系统定位结果进行融合，有效解决使用单一传感器定位精度及可靠性不高的问题。通过上述方法，本发明为车辆在隧道行驶过程中提供更准确的位置信息，提高了车辆行驶的安全性。

附图说明

图1是本发明提供优选实施例一种隧道内车辆融合定位系统架构

图2是本发明隧道内车辆融合定位流程图

图3是本发明图像匹配定位方法流程图

图4是本发明融合定位方法结构框图

图5是本发明离线图像地图的构建流程图

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

本发明提出一种基于车路通信的隧道内车辆融合定位系统及方法，包括路侧单元与车载定位设备，其中路侧单元包括离线图像地图模块、图像匹配模块和通信模块。离线图像地图模块用于存储隧道内的离线图像地图；通信模块用于接收车载定位设备预处理过的图像和发送图像匹配定位结果；图像匹配模块将接收到的图像与离线图像地图中的图像进行特征匹配从而计算图像采集点与离线图像地图数据点之间的相对位置，然后根据两者的相对位置和离线图像地图数据点的实际位置计算出图像采集点的位置。车载定位设备包括图像预处理模块、通信模块、惯性导航模块和融合定位模块。图像预处理模块将实时采集的图像进行畸变矫正和图像增强处理；通信模块用于车载定位设备与路侧单元之间的通信；惯性导航模块根据惯性测量单元的测量数据计算车辆当前位置信息；融合定位模块将惯性导航模块计算出的位置信息和图像匹配定位模块计算出的位置信息进行融合，从而获得更加精确的定位结果。

以下结合附图和具体实例对本发明的具体实施方式进行描述。

1.如图1所示为基于图像匹配的车辆隧道定位系统总体框架。包括路侧单元与车载定位设备，其中路侧单元包括离线图像地图模块、图像匹配模块和通信模块。离线图像地图模块用于存储隧道内的离线图像地图；通信模块用于接收车载定位设备预处理过的图像和发送图像匹配定位结果；图像匹配模块将接收到的图像与离线图像地图中的图像进行特征匹配从而计算图像采集点与离线图像地图数据点之间的相对位置，然后根据两者的相对位置和离线图像地图数据点的实际位置计算出图像采集点的位置。车载定位设备包括图像预处理模块、通信模块、惯性导航模块和融合定位模块。图像预处理模块将实时采集的图像进行畸变矫正和图像增强处理；通信模块用于车载定位设备与路侧单元之间的通信；惯性导航模块根据惯性测量单元的测量数据计算车辆当前位置信息；融合定位模块将惯性导航模块计算出的位置信息和图像匹配定位模块计算出的位置信息进行融合，从而获得更加精确的定位结果。

2.如图2所示为本发明的实施流程，其步骤包括：

(1)检测车辆是否接近隧道。若车辆未靠近隧道则退出，若车辆靠近隧道则进行车载定位设备与路侧单元之间的通信连接。

(2)检测车辆是否进入隧道，若车辆未进入隧道则退出，若车辆进入隧道则车载定位设备采集图像并进行预处理，然后将预处理过的图像通过通信模块发送给路侧单元，由路侧单元完成图像匹配定位并将结果发送给车载定位设备。

(3)车载定位设备采集惯性导航模块的数据，计算车辆位置。

(4)根据图像匹配定位结果与惯性导航模块数据进行融合定位。

(5)重复上述过程直到根据离线图像地图数据判定车辆已驶出隧道。

3.如图3所示为本发明提出的图像匹配定位方法流程，该方法包括以下步骤：

(1)图像采集及图像预处理。车载定位设备通过车载双目相机采集车辆前方图像并进行畸变矫正和图像增强处理；

(2)路测设备对采集的图像进行特征提取。以图像的ORB(Oriented FAST andRotated BRIEF)特征为例说明本专利方法流程，但不限于该特征；

(3)双目图像特征匹配。将双目相机采集的左右两幅图像特征匹配，以特征点间的汉明距离小于最小汉明距离的两倍为依据对特征点进行筛选；

(4)根据双目图像视差计算特征点深度。根据步骤(3)中的匹配结果和特征点的像素坐标，计算特征点深度，得到像素点的空间坐标；

(5)获取离线图像地图中的图像并提取特征。根据惯性导航系统提供的位置信息确定离线图像地图中的数据点q_i。选取数据点q_i及其前后数据点q_i-1和q_i+1中的图像并提取这三张图像ORB特征；

(6)实时图像与离线图像地图中的图像进行特征匹配。将采集的图像和(5)中的三张图像进行特征匹配，以特征点间的汉明距离小于最小汉明距离的两倍为依据对特征点进行筛选。选择特征匹配点数最多的两幅图像计算图像采集点的位置；

(7)求解旋转矩阵R和平移向量t。根据(6)中图像匹配结果利用PnP的原理求解图像采集点与离线图像地图数据点之间的旋转矩阵R和平移向量t；

(8)计算车辆位置。根据步骤(7)可以算出图像采集点相对于离线图像地图数据点之间的旋转矩阵R和平移向量t。离线图像地图数据点中存有该数据点的实际位置。由此，可以由旋转矩阵R、平移向量t和离线图像地图数据点的实际位置推算出车辆当前的位置。该位置信息以经纬度的方式表示。

4.双目特征匹配的具体过程为：

(1)左目图像的特征点集为P_l＝{p_l,1,p_l,2,…,p_l,m}，右目图像的特征点集为P_r＝{p_r,1,p_r,2,…,p_r,m}。其中特征点p_i对应的描述子为d_i。

(2)根据特征点描述子之间的汉明距离进行特征点集P_l和P_r之间的匹配。

(3)找出各组匹配中的最小汉明距离H_min。

(4)以特征点间的汉明距离小于最小汉明距离H_min的两倍为依据对匹配的特征点对进行筛选，消除错误匹配。

5.根据双目图像视差计算特征点深度的具体过程为：

(1)左目图像特征点像素横坐标为u_l,i(i＝1..n)，与其对应的右目图像特征点像素横坐标为u_r,i(i＝1..n)。

(2)左右目图像特征点之间的视差为d_i＝u_l,i-u_r,i。

(3)左右目相机之间的基线长度为b，左右目相机焦距均为f。

(4)由相机模型及三角形相似原理可得，特征点的深度为：

(5)根据相机的内参和特征点的像素可以确定特征点的空间坐标p_i＝(x_i,y_i,z_i)。

6.实时图像与离线图像地图中的图像进行特征匹配的具体过程为：

(1)采集图像的特征点集为P＝{p₁,p₂,…,p_n}，离线图像地图数据点q_i、q_i+1和q_i-1处的特征点集分别为P_i＝{p_i,1,p_i,2,…,p_i,m}、P_i+1＝{p_i+1,1,p_i+1,2,…,p_i+1,k}和P_i-1＝{p_i-1,1,p_i-1,2,…,p_i-1,j}。其中特征点p_i对应的描述子为d_i。

(2)根据特征点描述子之间的汉明距离进行特征点集P与P_i-1、P_i和P_i+1之间的匹配。

(3)找出各组匹配中的最小汉明距离H_min。

(4)以特征点间的汉明距离小于最小汉明距离H_min的两倍为依据对匹配的特征点对进行筛选，消除错误匹配。

(5)在P与P_i、P与P_i+1和P与P_i-1这三组匹配结果中选择匹配点数最多的两组计算图像采集点的位置。

7.求解旋转矩阵R和平移向量t的具体过程为：

(1)根据相机的内参确定离线图像地图中图像特征点的归一化坐标p_mi＝(x_mi,y_mi,1)^T。

(2)将实时采集的图像特征点对应的空间坐标转换为齐次坐标p_i＝

(x_i,y_i,z_i,1)^T。

(3)根据相机的投影模型、归一化坐标p_mi和齐次坐标p_i计算旋转矩阵R和平移向量t。令T＝[R|t],则有

根据该方程，最少通过六对匹配点即可求解出旋转矩阵R和平移向量t。

8.图像匹配模块计算车辆位置的具体过程为：

(1)根据计算出的旋转矩阵R、平移向量t和车辆的航向角

来计算车辆在正东方向位置偏移量Δx和正北方向的位置偏移量Δy。

(2)根据步骤(1)中的偏移量Δx和Δy以及相应离线图像地图数据点的经纬度信息来计算车辆的当前的位置数据，该位置以经纬度的方式进行表示。

9.如图4所示为本发明融合定位模块结构框图。其融合滤波过程步骤包括：

(1)以惯性导航定位数据为子滤波器(LF1)的测量值进行Kalman滤波，得到位姿估计

及其协方差矩阵P₁。根据惯性导航模块输出的数据来计算车辆的位姿，需要对惯性导航模块输出的加速度和角速度进行处理。处理的具体过程为：

a.对加速度和角速度按时间进行积分，得到位置变化量和角度变化量。其中积分时长δt为相机采集两帧图像的间隔时长，Δp_w为地球坐标系下位置变化量，Δq_w为四元数形式的角度变化量，

为地球坐标系下第i时刻的速度，

为IMU机体坐标系下加速度测量值，g^w，为重力加速度，

为IMU机体坐标系下角速度测量值，

表示四元数乘法。

b.根据主滤波器反馈的上一时刻车辆位置和航向角和(a)中得到的位置变化量和角度变化量来计算车辆的当前位置和航向。

(2)以图像匹配定位数据为子滤波器(LF2)的测量值进行Kalman滤波，得到位姿估计

及其协方差矩阵P₂。

(3)将步骤(1)和步骤(2)中各子滤波器的位姿估计及其协方差矩阵送入到主滤波器并和主滤波器的估计一起进行融合得到全局最优的位姿估计

及其协方差矩阵P_f。

(4)以步骤(3)中获得的全局估计

及其协方差矩阵P_f来重置各子滤波器的估计和协方差矩阵。

滤波过程具体实现方式如下:

设X_k＝[x_k,y_k,θ_k]为第k时刻的状态向量，其中x_k和y_k表示所在位置的经纬度，θ_k第k时刻的航向角。P_k为第k时刻状态的协方差矩阵。则：

各子滤波器的状态转移函数为：

X_k+1＝Φ_k+1,kX_k+Γ_k+1w_k

其中Φ_k+1,k为系统状态转移矩阵，Γ_k+1为系统噪声矩阵，w_k是均值为零的白噪声序列，其协方差矩阵为Q_k。

各子滤波器的观测方程为：

Z_k＝H_kX_k+v_k

其中H_k是测量矩阵，测量噪声矩阵v_k是均值为零、方差为R_k的白噪声序列。

各子滤波器进行时间更新：

计算Kalman增益：

K＝P_k'₊₁H_k(H_kP_k'₊₁H^T+R_k)^-1

根据测量值和Kalman增益计算滤波结果：

P_k＝(I-KH_k)P′_k

在主滤波器中，对两个子滤波器的滤波结果进行融合，得到全局最优估计。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

10.如图5所示为本发明离线图像地图的构建流程。离线图像地图是本发明中最重要的组成部分之一。其步骤包括：

(1)在隧道中等间距地布置大小为M*N图像标签，所述图像标签在该隧道内具有唯一性；

(2)在隧道内设置离线图像地图数据点。各个数据点间的距离可以根据定位精度要求进行调整；

(3)在各个数据点处采集图像；

(4)保存各个数据点处的图像和位置信息，该位置信息以经纬度的方式进行表示。收集完隧道内所有数据点的图像和位置信息即完成了该隧道内离线图像地图的构建；

(5)将构建的离线图像地图存储到路侧单元中。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (9)

1.一种基于车路通信的隧道内车辆融合定位系统，其特征在于，包括：路侧单元与车载定位设备，其中路侧单元包括离线图像地图模块、图像匹配模块和路侧通信模块，离线图像地图模块与图像匹配模块相连接，图像匹配模块与路侧通信模块相连接；离线图像地图模块用于存储隧道内的离线图像地图；图像匹配模块将接收到的图像与离线图像地图中的图像进行特征匹配，从而计算图像采集点与离线图像地图数据点之间的相对位置，然后根据两者的相对位置和离线图像地图数据点的实际位置计算出图像采集点的位置；路侧通信模块用于接收车载定位设备预处理过的图像和发送图像匹配定位结果；

车载定位设备包括图像预处理模块、车载通信模块、惯性导航模块和融合定位模块；惯性导航模块和融合定位模块相连接，所述车载通信模块通过通信网络和路侧通信模块进行数据传输，图像预处理模块将实时采集的图像进行畸变矫正和图像增强处理；车载通信模块用于车载定位设备与路侧单元之间的通信；惯性导航模块根据惯性测量单元的测量数据计算车辆当前位置信息；融合定位模块将惯性导航模块计算出的位置信息和图像匹配定位模块计算出的位置信息进行融合，从而获得更加精确的定位结果。

2.根据权利要求1所述的一种基于车路通信的隧道内车辆融合定位系统，其特征在于，所述图像匹配定位模块对实时采集的图像和离线图像地图中的图像进行特征匹配从而计算图像采集点与离线图像地图数据点之间的相对位置，然后根据图像采集点与离线图像地图数据点之间的相对位置和离线图像地图数据点的实际位置计算出图像采集点的位置，其步骤包括：

(1)图像采集及图像预处理，车载定位设备通过车载双目相机采集车辆前方图像并进行畸变矫正和图像增强处理；

(2)路侧单元对采集的图像进行特征提取；

(3)双目图像特征匹配，将双目相机采集的左右两幅图像特征匹配，以特征点间的汉明距离小于最小汉明距离的两倍为依据对特征点进行筛选；

(4)根据双目图像视差计算特征点深度，根据步骤(3)中的匹配结果和特征点的像素坐标，计算特征点深度，得到像素点的空间坐标；

(5)获取离线图像地图中的图像并提取特征，根据惯性导航系统提供的位置信息确定离线图像地图中的数据点q_i，选取数据点q_i及其前后数据点q_i-1和q_i+1中的图像并提取这三张图像ORB特征；

(6)实时图像与离线图像地图中的图像进行特征匹配，将采集的图像和步骤(5)中的三张图像进行特征匹配，以特征点间的汉明距离小于最小汉明距离的两倍为依据对特征点进行筛选。选择特征匹配点数最多的两幅图像计算图像采集点的位置；

(7)求解旋转矩阵R和平移向量t，根据步骤(6)中图像匹配结果利用PnP(Perspective-n-Point)的原理求解图像采集点与离线图像地图数据点之间的旋转矩阵R和平移向量t；

(8)计算车辆位置，根据步骤(7)可以算出图像采集点相对于离线图像地图数据点之间的旋转矩阵R和平移向量t，离线图像地图数据点中存有该数据点的实际位置p＝(p_lo,p_la)^T和姿态角

由此，可以由旋转矩阵R、平移向量t和离线图像地图数据点的实际位置p和姿态

推算出车辆当前的位置

该位置信息以经纬度的方式表示。

3.根据权利要求2所述的一种基于车路通信的隧道内车辆融合定位系统，其特征在于，所述步骤(3)双目特征匹配的具体过程为：

(1)左目图像的特征点集为P_l＝{p_l,1,p_l,2,…,p_l,m}，右目图像的特征点集为P_r＝{p_r,1,p_r,2,…,p_r,m}，其中特征点p_i对应的描述子为d_i；

(2)根据特征点描述子之间的汉明距离进行特征点集P_l和P_r之间的匹配；

(3)找出各组匹配中的最小汉明距离H_min；

(4)以特征点间的汉明距离小于最小汉明距离H_min的两倍为依据对匹配的特征点对进行筛选，消除错误匹配。

4.根据权利要求2所述的一种基于车路通信的隧道内车辆融合定位系统，其特征在于，所述步骤(4)根据双目图像视差计算特征点深度的具体过程为：

(1)左目图像特征点像素横坐标为u_l,i(i＝1..n)，与其对应的右目图像特征点像素横坐标为u_r,i(i＝1..n)；

(2)左右目图像特征点之间的视差为d_i＝u_l,i-u_r,i；

(3)左右目相机之间的基线长度为b，左右目相机焦距均为f；

(4)由相机模型及三角形相似原理可得，特征点的深度为：

(5)根据相机的内参矩阵K和特征点的像素(u_i,v_i)^T确定特征点的空间坐标

其中内参矩阵K可通过相机标定得到。

5.根据权利要求2所述的一种基于车路通信的隧道内车辆融合定位系统，其特征在于，所述步骤(7)求解旋转矩阵R和平移向量t的具体过程为：

(1)根据相机的内参确定离线图像地图中图像特征点的归一化坐标

p_mi＝(x_mi,y_mi,1)^T；

(2)将实时采集的图像特征点对应的空间坐标转换为齐次坐标p_i＝(x_i,y_i,z_i,1)^T；

(3)根据相机的投影模型、归一化坐标p_mi和齐次坐标p_i计算旋转矩阵R和平移向量t，令T＝[R|t],则有

T由旋转矩阵和平移向量组成，表示两点之间的位姿变换关系，根据该方程，最少通过六对匹配点即可求解出旋转矩阵R和平移向量t。

6.根据权利要求2所述的一种基于车路通信的隧道内车辆融合定位系统，其特征在于，图像匹配模块计算车辆位置的具体过程为：

(1)根据离线图像地图数据点中存有该数据点的实际位置p＝(p_lo,p_la)^T和姿态角

计算该数据点的位姿矩阵

(2)根据计算出的旋转矩阵R、平移向量t计算车辆当前位置与离线图像地图数据点之间的相对位姿矩阵

(3)车辆当前的位姿矩阵

则车辆的当前位置为

7.根据权利要求1-6之一所述的一种基于车路通信的隧道内车辆融合定位系统，其特征在于，所述融合定位模块结构的滤波过程步骤包括：

(1)以惯性导航定位数据为子滤波器(LF1)的测量值进行Kalman滤波，得到位姿估计

及其协方差矩阵P₁，根据惯性导航模块输出的数据来计算车辆的位姿，需要对惯性导航模块输出的加速度和角速度进行处理；

(2)以图像匹配定位数据为子滤波器(LF2)的测量值进行Kalman滤波，得到位姿估计

及其协方差矩阵P₂；

(3)将步骤(1)和步骤(2)中各子滤波器的位姿估计及其协方差矩阵送入到主滤波器并和主滤波器的估计一起进行融合得到全局最优的位姿估计

及其协方差矩阵P_f；

(4)以步骤(3)中获得的全局估计

及其协方差矩阵P_f来重置各子滤波器的估计和协方差矩阵。

8.根据权利要求1-6之一所述的一种基于车路通信的隧道内车辆融合定位系统，其特征在于，离线图像地图模块的构建步骤包括：

(1)在隧道中等间距地布置大小为M*N图像标签，所述图像标签在该隧道内具有唯一性；

(2)在隧道内设置离线图像地图数据点。各个数据点间的距离可以根据定位精度要求进行调整；

(3)在各个数据点处采集图像；

(4)保存各个数据点处的图像和位置信息，该位置信息以经纬度的方式进行表示。收集完隧道内所有数据点的图像和位置信息即完成了该隧道内离线图像地图的构建；

(5)将构建的离线图像地图存储到路侧单元中。

9.一种基于权利要求1-8之一系统的融合定位方法，其特征在于，包括以下步骤：

(1)检测车辆是否接近隧道，若车辆未接近隧道则退出，若车辆靠近隧道则进行车载定位设备与路侧单元之间的通信连接；

(2)检测车辆是否进入隧道，若车辆未进入隧道则退出，若车辆进入隧道则车载定位设备采集图像并进行预处理，然后将预处理过的图像通过通信模块发送给路侧单元，由路侧单元完成图像匹配定位并将结果发送给车载定位设备；

(3)车载定位设备采集惯性导航模块的数据，计算车辆位置；

(4)根据图像匹配定位结果与惯性导航模块数据进行融合定位；

(5)重复上述过程直到根据离线图像地图数据判定车辆已驶出隧道。

Images