[发明专利]基于视觉和毫米波雷达地图特征的隧道内车辆定位方法

权利要求书

1.一种基于视觉和毫米波雷达地图特征的隧道内车辆定位方法，其特征在于，将感知特征点数据集与高精度地图层数据相融合，使无人驾驶车辆在光照暗和定位信号弱的环境下行驶时，根据从地图端提取融合的数据与GPS数据，推算出精确的车身位置。

2.根据权利要求1中任一所述的基于视觉和毫米波雷达地图特征的隧道内车辆定位方法，其特征在于，所述的感知特征点数据集为立体视觉与毫米波雷达检测出的特征点数据元经过配准融合而形成。

3.根据权利要求2所述的基于视觉和毫米波雷达地图特征的隧道内车辆定位方法，其特征在于，所述的立体视觉提取的目标包括：车道线、路沿、隧道口和信号灯柱，所述的毫米波雷达探测各种静态目标，包括：信号灯柱和路灯柱。

4.根据权利要求2所述的基于视觉和毫米波雷达地图特征的隧道内车辆定位方法，其特征在于，所述的车辆定位方法包括以下步骤：

步骤1、立体视觉相机重建场景和特征点提取；

步骤2、毫米波雷达的特征点提取；

步骤3、毫米波雷达数据与图像数据配准；

步骤4、配准后的特征点数据集融合到地图；

步骤5、行驶时从地图中的特征点推导出自身位置。

5.根据权利要求4所述的基于视觉和毫米波雷达地图特征的隧道内车辆定位方法，其特征在于，所述的步骤1中的立体视觉相机重建场景包括：立体视觉相机重建场景采用双目立体视觉的方案，以SLAM算法为基础，将深度图中的平面信息加入帧间配准算法，使用截断符号距离函数体重建方法。

6.根据权利要求4所述的基于视觉和毫米波雷达地图特征的隧道内车辆定位方法，其特征在于，所述的步骤1中的特征点提取为从图像帧数据中提取出车道线、路沿、信号灯柱和隧道口的静态目标的角点即特征点，包括以下步骤：

步骤1.1、通过前一帧图像求解出当前帧的旋转矩阵R和平移向量t，即相对于前一帧的姿态和位移；

步骤1.2、像素坐标系到世界坐标系的转换，假设空间点P在世界坐标下的齐次坐标是(Xw,Yw,Zw,1)^T，相机坐标下的齐次坐标是(Xc,Yc,Zc,1)^T，图像坐标是(x,y),像素坐标为(u,v)，在图像坐标系下，一个像素在x轴和y轴方向上的物理尺寸为d_x,d_y；图像中心点为像素原点(u₀,v₀)，像素坐标与图像坐标之间的转换关系为：

若f为相机的焦距，则图像坐标与相机坐标的转换关系为；

基于R和T，相机坐标系与世界坐标系的转换关系为；

所述的提取算法基于深度学习的框架并采用RANSAC算法加以优化，消除掉错误的特征匹配点。

7.根据权利要求4所述的基于视觉和毫米波雷达地图特征的隧道内车辆定位方法，其特征在于，所述的步骤2为从雷达扫描帧数据中提取出信号灯柱的相对距离、角度等并解算出位置，所述的标的物的位置是一个世界坐标系下的二维坐标(X,Y)。

8.根据权利要求4所述的基于视觉和毫米波雷达地图特征的隧道内车辆定位方法，其特征在于，所述的步骤3具体包括：

步骤3.1、毫米波雷达数据与图像数据的配准关系用一个2X3的矩阵R表示；

步骤3.2、依据相机的成像原理，若空间中某一个点M在相机坐标系下的齐次坐标为Mc(Xc,Yc,Zc)，雷达探测到的同一个点投影到图像坐标下的坐标为Mr(Xr,Yr)，存在这样的关系：Mr＝R*Mc；

步骤3.3、若已知坐标Mr和Mc，就得到毫米波雷达数据与图像数据之间的配准关系R。