CN108802785A - 基于高精度矢量地图和单目视觉传感器的车辆自定位方法 - Google Patents

Abstract

本发明涉及一种基于高精度矢量地图和单目视觉传感器的车辆自定位方法，其步骤：设置一包括GNSS模块、地图模块、姿态估计模块、预估图像模块、单目视觉模块、图像处理模块和高精度匹配算法模块的定位系统；姿态估计模块根据GNSS提供的初始位置和地图信息，估计车辆的初始位姿；预估图像模块根据地图模块提供的地理位置以及姿态估计模块提供的初始位姿，生成预估图像；单目视觉模块将采集的道路原始信息传输至图像处理模块，图像处理模块处理图像信息，得到用于地图匹配的特征；高精度匹配算法模块根据接收到的用于地图匹配的特征和预估图像中的特征匹配程度，获得基于地图匹配解算的车辆位置及姿态信息，得到对车辆位置和姿态的估计。

Description

基于高精度矢量地图和单目视觉传感器的车辆自定位方法

技术领域

本发明涉及一种智能汽车到导航定位领域，特别是关于一种在城市交通场景下，基于高精度矢量地图和单目视觉传感器的车辆自定位方法。

背景技术

在智能汽车系统中，车辆高精度的自定位技术是实现车辆决策、控制的重要前提。现有车辆定位方法中，传统卫星定位(GNSS)精度难以满足智能汽车需求；RTK-GPS技术虽能在空旷环境中实现较高精度定位，但是在城市复杂场景中，因信号遮挡或反射，使得该方法精度不够理想；航位推算(DR)技术与GNSS结合的方法虽然能一定程度上弥补卫星定位的误差，但是在长期信号不稳定的情况下，误差累积效应使得该方法不能满足智能汽车城市场景定位需求。

地图匹配技术是另一种车辆自定位技术，依靠车载传感器和事先采集的地图进行匹配，从而得到车辆在地图中的位置。以该方法为原理的高精度定位技术中，使用激光雷达传感器的匹配算法是常用方法，但是激光雷达成本较高，对相应地图的数据量要求非常大，使得匹配算法对动态环境的变化(如其他车辆遮挡)较敏感。而结合多目立体视觉、深度相机等测距传感器的地图匹配技术，虽然成本相对较低，但测距精度较低导致自定位效果较差，且同样无法适应车辆形式场景的动态变化。

发明内容

针对上述问题，本发明的目的是提供一种基于高精度矢量地图和单目视觉传感器的车辆自定位方法，在满足智能汽车高精度定位需求(通常分米级)的同时，降低定位系统的成本，提高车辆在城市动态变化场景中定位的鲁棒性。

为实现上述目的，本发明采取以下技术方案：一种基于高精度矢量地图和单目视觉传感器的车辆自定位方法，其包括以下步骤：1)设置一定位系统，该定位系统包括GNSS模块、地图模块、姿态估计模块、预估图像模块、单目视觉模块、图像处理模块和高精度匹配算法模块；2)GNSS模块向姿态估计模块提供地图匹配的初始位置，姿态估计模块根据GNSS提供的初始位置和地图信息，估计车辆的初始位姿；3)地图模块向预估图像模块提供道路特征的地理位置；预估图像模块根据地图模块提供的地理位置以及姿态估计模块提供的初始位姿，生成预估图像，并传输至高精度匹配算法模块；4)单目视觉模块将采集的道路原始信息传输至图像处理模块，图像处理模块处理图像信息，得到用于地图匹配的特征，并传输至高精度匹配算法模块；5)高精度匹配算法模块根据接收到的用于地图匹配的特征和预估图像中的特征匹配程度，获得基于地图匹配解算的车辆位置及姿态信息，得到对车辆位置和姿态的估计。

进一步，所述步骤2)中，GNSS模块得到车辆粗略位置后，根据GNSS天线与相机的位置关系，得到相机中心在地图坐标系中粗略的位置估计，同时，读取地图中该位置的地面方向和车道方向信息，根据“相机平行于地面”、“相机平行于车道线”，得出相机坐标系的三个坐标轴在地图坐标系中的方向，并据此得到相机的初始姿态，从而得到车辆初始的位姿估计。

进一步，所述步骤3)中，在预估图像模块中，根据位姿估计，从地图中提取相机能看到的特征，并根据相机姿态P⁰ _C和相机成像模型，将这些特征投影到预估图像中。

进一步，所述步骤4)中，在图像处理模块中，具体处理过程如下：4.1)通过机器学习的方法，对图像的每个像素进行分类；4.2)然后依据某一几何特征抽象的方法，对同一语义的区域进行几何对象的拟合。

进一步，所述步骤5)中，根据由预估位姿得到的预估图像和单目视觉传感器采集后通过图像处理模块得到的对象，通过评价两者的相似程度，进而构成目标函数，通过优化预估位姿，当预估图像中的特征和单目视觉中的对象相似程度取得最高即目标函数最小时，预估位姿收敛到真实位姿，求得车辆的位置和姿态。

进一步，在计算目标函数时，对每一个直线段对象，找到与其语义相同的，最近的预估直线段特征，计算直线段的代价；对每一个曲线段对象，找到与其最近的同语义预估曲线段特征，计算曲线段的代价；找到点对象最近的同语义预估点特征为，计算点对象的代价。

本发明由于采取以上技术方案，其具有以下优点：1、本发明可实现分米级(30cm左右)的高精度定位，满足智能汽车的需求。2、本发明只需单目视觉传感器和提供参考的一般精度低成本卫星定位系统，大大降低车辆自定位系统的成本。3、本发明通过单目视觉对环境的语义级理解，提高了相关算法对高动态交通环境的适应性。4、本发明较现有基于地图匹配的高精度定位方法，该方案对地图的数据量要求大大降低，从而降低实际应用中地图传输、存储和管理的成本。

附图说明

图1是本发明的整体流程示意图；

图2是相机姿态描述示意图；

图3是预估图像示意图；

图4是图像处理模块示意图；

图5是语义级识别结果；

图6是图像几何特征识别示意图；

图7是地图匹配代价函数计算示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明提供一种基于高精度矢量地图和单目视觉传感器的车辆自定位方法，其包括以下步骤：

1)设置一定位系统，该定位系统包括GNSS模块、地图模块、姿态估计模块、预估图像模块、单目视觉模块、图像处理模块和高精度匹配算法模块；

2)GNSS模块向姿态估计模块提供地图匹配的初始位置，姿态估计模块根据GNSS提供的初始位置和地图信息，估计车辆的初始位姿；

3)地图模块向预估图像模块提供道路特征的地理位置；预估图像模块根据地图模块提供的地理位置以及姿态估计模块提供的初始位姿，生成预估图像，并传输至高精度匹配算法模块；

4)单目视觉模块将采集的道路原始信息传输至图像处理模块，图像处理模块处理图像信息，得到用于地图匹配的特征，并传输至高精度匹配算法模块；

5)高精度匹配算法模块根据接收到的用于地图匹配的特征和预估图像中的特征匹配程度，获得基于地图匹配解算的车辆位置及姿态信息，得到对车辆位置和姿态的估计。

上述步骤2)中，如图2所示，地图坐标系用O_W-X_WY_WZ_W表示，相机坐标系用O_C-X_CY_CZ_C表示，相机在地图坐标中的位置描述用P_C＝(X_CW,Y_CW,Z_CW,α_CW,β_CW,γ_CW)表示，其中X_CW,Y_CW,Z_CW为相机中心在地图坐标系中的坐标，α_CW,β_CW,γ_CW为将大地坐标系先后绕O_WX_W、O_WY_W、O_WZ_W轴按右手定则旋转至与相机坐标系方向平行时转过的三个角度。GNSS模块得到车辆粗略位置后，根据GNSS天线与相机的位置关系，得到相机中心在地图坐标系(O_W-X_WY_WZ_W)中粗略的位置估计(X⁰ _CW,Y⁰ _CW,Z⁰ _CW)，同时，读取地图中该位置的地面方向和车道方向信息，根据“相机平行于地面”、“相机平行于车道线”，得出相机坐标系(O_C-X_CY_CZ_C)的三个坐标轴在地图坐标系中的方向，并据此得到相机的初始姿态(α⁰ _CW,β⁰ _CW,γ⁰ _CW)，从而得到车辆初始的位姿估计P⁰ _C＝(X⁰ _CW,Y⁰ _CW,Z⁰ _CW,α⁰ _CW,β⁰ _CW,γ⁰ _CW)，其中α⁰ _CW表示相机的俯仰角，β⁰ _CW表示相机的偏航角，γ⁰ _CW表示相机的滚转角。

上述步骤3)中，在预估图像模块中，根据位姿估计，从地图中提取相机可能看到的特征，并根据相机姿态P⁰ _C和相机成像模型，将这些特征投影到预估图像中。例如在一个实例中，根据相机位置和方向，预估在距离相机中心70m范围、位于相机光轴正向(即Z_CW正坐标)的地图内特征为预估可见特征{F_i|i＝1,2,...,N}，其中F_i为第i个特征，其第j个控制点位置为X_ij＝(X_Wij,Y_Wij,Z_Wij)。在该实例中，用针孔成像模型将所有特征的所有控制点按照以下方程投影到预估图像中。

其中，u_ij v_ij为第i个特征的第j个控制点在预估图像中的坐标；I为相机内参，通过相机标定即可获知；

初始旋转矩阵R通过下式计算：

初始平移矩阵t通过下式进行计算：

t＝-R(X⁰ _CW,Y⁰ _CW,Z⁰ _CW)^T。

通过上述实例，计算出了特征的控制点在预估图像中的位置，得到的预估图像示意图如图3所示。其中，为预估直线段特征集合，式中为线段的两个端点坐标，s_i为该对象的语意属性，在该实例中 为预估曲线段特征集合，其中式中为曲线上的控制点坐标，s_i为该对象的语意属性，在该实例中 为预估点特征集合，其中式中为该中心点的位置坐标，为该对象的语意属性，在该实例中，

上述步骤4)中，在图像处理模块中，具体处理过程如下：

4.1)通过机器学习的方法，对图像的每个像素进行分类。

在一个实例中，通过PSPnet网络，以城市典型数据集对网络进行训练，网络计算出每个像素从属于某个语义种类的概率，并输出概率最大的语义。如图4所示，语义分类有车道线、交通标志牌、交通灯、交通灯杆、树、路灯杆等。像素级语义分类的结果如图5所示，其中，1、2为路灯杆，3为交通灯杆，4、5、6为车道线，7、9为交通灯，8位交通标志牌。

4.2)然后依据某一几何特征抽象的方法，对同一语义的区域进行几何对象的拟合。

在一个实例中，对于路灯杆、交通灯杆等语义区域，用RANSAC方法进行线段的拟合，得到线段对象集合L＝{L_i|i＝1,...,N}，其中L_i＝(p_i1,p_i2,s_li)，p_i1p_i1为线段的两个端点坐标，s_i为该对象的语意属性，在该实例中如图6所示，拟合出L₁L₂L₃三个直线段对象。对于车道线、交通标志牌等语义区域，用边缘提取的方法，得到曲线段对象集合C＝{C_i|i＝1,...,N}，其中C_i＝(p_i1,...,p_in,s_Ci)，式中p_i1,...,p_in为曲线上的控制点坐标，s_i为该对象的语意属性，在该实例中如图6中，拟合出C₁C₂C₃C₄四个曲线段对象。对于交通标志牌、交通灯等语义区域，通过计算区域形心，计算区域中心点，得到点对象集合P＝{P_i|i＝1,...,N}，其中P_i＝(p_i,s_pi)，式中p_i为该中心点的位置坐标，s_pi为该对象的语意属性，在该事例中，如图6中，拟合出P₁P₂P₃三个点对象。

上述步骤5)中，根据前述由预估位姿得到的预估图像和单目视觉传感器采集后通过图像处理模块得到的对象，可以通过评价两者的相似程度，进而构成目标函数，通过优化预估位姿，当预估图像中的特征和单目视觉中的对象相似程度取得最高(目标函数最小)时，预估位姿收敛到真实位姿，即求得车辆的位置和姿态。

在一个实例中，优化参数为单目相机在地图中的位置P_C的六个参数，优化迭代的初始值为前述预估初始位置P⁰ _C＝(X⁰ _CW,Y⁰ _CW,Z⁰ _CW,α⁰ _CW,β⁰ _CW,γ⁰ _CW)。以某种适当的方式，根据预估图像中的预估特征和单目视觉中提取的对象的位置和语义信息，定义优化的目标函数为在一个实例中，如图5所示，代价的计算方法为：L₁为单目视觉中的直线段对象，在本实例中，在L₁上均匀选取5个控制点2.1,2.2,2.3，2.4,2.5。C₁为单目视觉中的曲线对象，在本实例中均匀选取5个控制点4.1,4.2,4.3,4.4,4.5，P₁为单目视觉中的点对象。为预估直线段对象，为预估曲线段，为预估点特征。

在计算目标函数时，对每一个直线段对象，找到与其语义相同的，最近的预估直线段特征，在本实例中，找到直线段对象L₁最近的同语义预估直线段特征为计算直线段的代价为其中，d_1i为第i个控制点到所在直线的距离，θ₁θ₂为L₁ 与水平方向的夹角，w₁w₂为距离和角度差异在总代价中的权重，在本实例中均取1。找到C₁最近的同语义预估曲线段特征为计算曲线段的代价为d_3i为第i个控制点到所在连续曲线的距离，该连续曲线由预估曲线段的控制点拟合确定，w₃为曲线段在总的代价中所占的权重，在本实例中取1。找到点对象P₁最近的同语义预估点特征为计算点对象的代价为e_P＝w₄d₅，其中d₅为点P₁与点的距离，w₄为点对象在总的代价中所占的权重，在本实例中取1。

在本实例中，优化目标函数为在求解优化问题中，用合适的优化算法求解使得目标函数最小的相机位姿。并以当前时刻结算的位置和姿态为初值，根据下一时刻的单目视觉处理结果，用同样的方法计算下一时刻的相机位姿，从而实现对车辆连续的高精度定位。

上述各实施例仅用于说明本发明，各个步骤都是可以有所变化的，在本发明技术方案的基础上，凡根据本发明原理对个别步骤进行的改进和等同变换，均不应排除在本发明的保护范围之外。

Claims (6)

1.一种基于高精度矢量地图和单目视觉传感器的车辆自定位方法，其特征在于，包括以下步骤：

1)设置一定位系统，该定位系统包括GNSS模块、地图模块、姿态估计模块、预估图像模块、单目视觉模块、图像处理模块和高精度匹配算法模块；

2)GNSS模块向姿态估计模块提供地图匹配的初始位置，姿态估计模块根据GNSS提供的初始位置和地图信息，估计车辆的初始位姿；

3)地图模块向预估图像模块提供道路特征的地理位置；预估图像模块根据地图模块提供的地理位置以及姿态估计模块提供的初始位姿，生成预估图像，并传输至高精度匹配算法模块；

4)单目视觉模块将采集的道路原始信息传输至图像处理模块，图像处理模块处理图像信息，得到用于地图匹配的特征，并传输至高精度匹配算法模块；

5)高精度匹配算法模块根据接收到的用于地图匹配的特征和预估图像中的特征匹配程度，获得基于地图匹配解算的车辆位置及姿态信息，得到对车辆位置和姿态的估计。

2.如权利要求1所述方法，其特征在于：所述步骤2)中，GNSS模块得到车辆粗略位置后，根据GNSS天线与相机的位置关系，得到相机中心在地图坐标系中粗略的位置估计，同时，读取地图中该位置的地面方向和车道方向信息，根据“相机平行于地面”、“相机平行于车道线”，得出相机坐标系的三个坐标轴在地图坐标系中的方向，并据此得到相机的初始姿态，从而得到车辆初始的位姿估计。

3.如权利要求1所述方法，其特征在于：所述步骤3)中，在预估图像模块中，根据位姿估计，从地图中提取相机能看到的特征，并根据相机姿态P⁰ _C和相机成像模型，将这些特征投影到预估图像中。

4.如权利要求1所述方法，其特征在于：所述步骤4)中，在图像处理模块中，具体处理过程如下：

4.1)通过机器学习的方法，对图像的每个像素进行分类；

4.2)然后依据某一几何特征抽象的方法，对同一语义的区域进行几何对象的拟合。

5.如权利要求1所述方法，其特征在于：所述步骤5)中，根据由预估位姿得到的预估图像和单目视觉传感器采集后通过图像处理模块得到的对象，通过评价两者的相似程度，进而构成目标函数，通过优化预估位姿，当预估图像中的特征和单目视觉中的对象相似程度取得最高即目标函数最小时，预估位姿收敛到真实位姿，求得车辆的位置和姿态。

6.如权利要求5所述方法，其特征在于：在计算目标函数时，对每一个直线段对象，找到与其语义相同的，最近的预估直线段特征，计算直线段的代价；对每一个曲线段对象，找到与其最近的同语义预估曲线段特征，计算曲线段的代价；找到点对象最近的同语义预估点特征为，计算点对象的代价。