CN114565674A - 自动驾驶车辆城市结构化场景纯视觉定位方法及装置 - Google Patents

Abstract

本发明公开了一种自动驾驶车辆城市结构化场景纯视觉定位方法及装置，该方法包括：设置并行的两线程：视觉SLAM定位线程，其用于采用基于局部地图的ORB特征点法实时获取车辆当前位姿；离线语义地图匹配定位线程，其包括：1)训练并检测相机获取的原始图像中的结构化语义标志物；2)根据地面先验，获取标志物在图像坐标系的二维信息；3)对标志物进行时域跟踪，当同一帧检测获取标志物数量不少于预设数时，构建车辆与标志物的二维拓扑图，其拓扑边权重由当前帧所关联的标志物可信度确定；4)根据车辆当前位置，依据二维拓扑图进行地图搜索匹配，恢复车辆当前二维位姿；5)将运行轨迹的信息和标志物信息上传至云端，更新离线地图语义信息库。

Description

自动驾驶车辆城市结构化场景纯视觉定位方法及装置

技术领域

本发明涉及自动驾驶技术领域，特别是关于一种基于先验地图的自动驾驶车辆城市结构化场景纯视觉定位方法及装置。

背景技术

AV(Autonomous vehicles；自动驾驶车辆)能够通过车载传感器解算自身位姿，并结合位姿信息进行自主规划及横纵向控制，其中必然使用定位方法。定位方法大体分为基于GNSS(Global Navigation Satellite System；全球导航卫星系统)的绝对定位和基于相机、激光雷达、IMU(Inertial Measurement Unit；惯性测量单元)等环境感知传感器的相对定位。

绝对定位通常依赖于GNSS的可用性，GNSS包含诸如GPS(Global PositionSystem；全球定位系统)、Glonass(global navigation satellite system；格洛纳斯系统)、北斗卫星导航系统和Galileo(Galileo satellite navigation system；伽利略定位系统)等。而环境或结构干扰容易导致GNSS信号丢失，现有技术往往通过将GNSS与IMU进行组合，导航定位，这样可可以在GNSS信号短暂缺失的情形下短暂地提供准确的定位信息。

相对定位则指使用相机、激光雷达、毫米波雷达和超声定位系统感知周遭环境，根据所提取到的环境特征点、线、面、距离及语义信息，实现AV的局部定位。

在AV众多定位方法中，视觉定位由于具有成本低、语义信息丰富的特点，受到了广大科研者的青睐。然而，作为相对定位的分支，视觉定位方法面临大范围未知场景时，往往存在较大的累计误差，且定位结果无法与全局地图构建关联。此外，单目视觉定位方法无法恢复出尺度信息，长时距定位存在较大尺度漂移，因而面对无GNSS信号的长距离定位任务，现有多数算法无法满足AV定位需求。如何获取持续、可靠的全局定位信息，成为了AV视觉定位实用化所面临的一大瓶颈。现有技术多采取关联全局地图的方式来解决这个问题，但是，长时、无GPS信号的复杂城市工况下的定位问题仍亟待解决。

发明内容

本发明的目的在于提供一种基于先验地图的自动驾驶车辆城市结构化场景纯视觉定位方法及装置来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种自动驾驶车辆城市结构化场景纯视觉定位方法，其包括：

设置并行的两线程：

一所述线程为视觉SLAM定位线程，其用于采用基于局部地图的ORB特征点法实时获取车辆当前位姿；

另一所述线程为离线语义地图匹配定位线程，其用于通过地面先验，恢复所述视觉SLAM定位线程所获取的特征点的尺度，其具体包括：

1)训练并检测相机获取的原始图像中的结构化语义标志物；

2)根据地面先验，获取所述标志物在图像坐标系的二维信息；

3)对所述标志物进行时域跟踪，当同一帧检测获取所述标志物数量不少于预设数目时，构建车辆与标志物的二维拓扑图，且其拓扑边权重由当前帧所关联的所述标志物可信度确定；

4)根据车辆当前位置，依据所述二维拓扑图进行地图搜索匹配，以恢复车辆当前二维位姿；

5)由连续时刻车辆二维位姿信息构建运行轨迹，将所述运行轨迹的信息和所述标志物信息上传至云端，更新离线地图语义信息库。

进一步地，所述步骤2)具体包括：

2.1)在同步运行的视觉SLAM定位线程中，提取ORB特征点，再采取三角化不断恢复无尺度约束的三维特征点的三维信息；

2.2)在每个标志物识别框内选择所需的标志物；

2.3)将步骤2.1)得到的三维特征点反投影到相机帧中，估计相关地平面参数并利用几何约束细化选取近地特征点；

2.4)使用滑动窗口策略跟踪连续多帧近地特征点，依照步骤2.3)选取的近地特征点策略再次使用RANSAC平面拟合法，获取最优地面点集并拟合地平面参数；

2.5)基于地面假设模型恢复2.4)获得的近地特征点的真实空间坐标，进而得到步骤2.1)获得的所有无尺度特征点的真实尺度，依此获取各标志物的三维信息。

进一步地，所述步骤2.2)具体包括：

2.21)计算每个标志物识别框内所述三维特征点到相机中心距离的均值

及标准差σ_d；

2.22)剔除满足如下条件之一的所述三维特征点：

a.所述三维特征点到相机中心距离大于所述均值

预设倍数的远点；

b.所述三维特征点到相机中心距离与所述均值

的差值最大的点；

2.23)返回步骤2.21)，直至识别框内的所述三维特征点的标准差σ_d小于预设距离值，最后丢弃三维特征点的数量小于预设数的标志物。

进一步地，所述步骤2.3)具体包括：

2.31)采用Hesse形式参数化表示地平面；

2.32)构建平面匹配模型

使用RANSAC算法拟合地平面；其中，

表示平面单位法向量，n_π1,n_π2,n_π3分别为单位法向量

在三轴的分量，X_i表示第i个所述三维特征点的三维空间坐标，d表示平面距离坐标系原点的距离；

2.33)计算步骤2.1)得到的三维特征点X_i到平面距离

判断d_i是否小于距离阈值d_threshold，如果是，则判定该三维特征点为近地特征点。

进一步地，所述步骤3)具体包括：

3.1)根据γ_l＝γ_d÷d，判断当前帧所关联标志物位置可信度γ_l，其中，γ_d表示根据YOLO-v5检测时网络直接输出的标志物检测置信度，d表示由步骤2)获得的各标志物的三维信息获取的标志物到当前帧之间的距离标志物距离；

3.2)保留当前帧所关联标志物的γ_l超过阈值γ_threshold的标志物，其余标志物进行时域跟踪，即后续连续时刻重复上述步骤3.1)到3.2)，直到该标志物满足γ_l>γ_threshold，期间，若该标志物连续三帧丢失，则丢弃该标志物；

3.3)当前帧所检测到的可信标志物数量超过预设数时，连接车辆位姿节点及标志物节点，构建二维拓扑图，并依据位置可信度赋予连接边权重，构建标志物拓扑连接图。

进一步地，所述步骤4)具体包括：

4.1)使用下式提供的匀速车辆模型获取当前时刻车辆初始位姿T_i：

T_i＝T_i-1 ²/T_i-2

式中，T_i-2、T_i-1分别表示前两个时刻的车辆位姿；

4.2)根据步骤4.1)所获取当前时刻车辆位姿T_i、步骤1)标志物检测所获取当前所观测到标志物类别，在预先下载好的NDS离线地图的相应层级中搜索同类别标志物信息及周围路网信息，并加载当前时刻初始位置T_i周围九宫格的地图块信息，并将该地图块信息于车端分配相应空间进行缓存；

4.3)依据步骤4.2)中的当前所观测到的标志物类别、当前时刻初始位置T_i进行搜索匹配，依据γ_l计算得到的地图特征置信度α_s；

4.4)选择置信度最高的两个标志物位置信息，恢复车辆当前二维位姿，返回步骤4.3)，直到定位结果收敛。

进一步地，所述步骤4.4)中，恢复车辆当前二维位姿的方法具体包括：

使用当前帧所选取的两个标志物相对于当前相机位姿的观测误差构建当前帧误差方程e＝[e₁ e₂]，使用奇异值分解求解e＝0，得到车辆当前二维位姿，其中，e₁和e₂通过以下误差方程获得：

式中，m取1、2分别表示当前帧所关联的两个标志物序号，

表示标志物m相对于当前相机位姿的径长观测误差，

表示标志物m相对于当前相机位姿的角度观测误差；w_m＝[x^m y^m]^τ表示标志物m在当前时刻相机坐标系下的真实位置；

表示标志物m的观测值，

分别表示标志物m的径长和角度的观测值；

表示观测方程；τ表示转置；上式由观测方程与观测值作差获得，求解得到当前帧的车辆二维位姿w_i。

本发明还提供一种自动驾驶车辆城市结构化场景纯视觉定位系统，包括：

视觉SLAM定位线程，其用于采用基于局部地图的ORB特征点法实时获取车辆当前位姿；

离线语义地图匹配定位线程，其用于通过地面先验，恢复所述视觉SLAM定位线程所获取的特征点的尺度，其具体包括：

标志物检测单元，其用于训练并检测相机获取的原始图像中的结构化语义标志物；

标志物二维信息获取单元，其用于根据地面先验，获取所述标志物在图像坐标系的二维信息；

拓扑构建单元，其用于对所述标志物进行时域跟踪，当同一帧检测获取标志物数量不少于预设数目时，构建车辆与标志物的二维拓扑图，且其拓扑边权重由当前帧所关联的标志物可信度确定；

位姿恢复单元，其用于根据车辆当前位置，依据所述二维拓扑图进行地图搜索匹配，以恢复车辆当前二维位姿；

信息更新单元，其用于由时序位姿信息构建运行轨迹，将运行轨迹的信息和所述标志物信息上传至云端，更新离线地图语义信息库。

进一步地，所述标志物二维信息获取单元具体包括：

特征点三维信息获取子单元，其用于在同步运行的视觉SLAM定位线程中，提取ORB特征点，再采取三角化不断恢复特征点三维信息，此时的特征点无尺度约束；

标志物选取子单元，其用于在每个标志物识别框内选择所需的标志物；

近地特征点选取子单元，其用于将特征点三维信息获取子单元得到的三维特征点反投影到相机帧中，估计相关地平面参数并利用几何约束细化选取近地特征点；

地平面参数拟合子单元，其用于使用滑动窗口策略跟踪连续多帧近地特征点，依照近地特征点选取子单元选取的近地特征点策略再次使用RANSAC平面拟合法，获取最优地面点集并拟合地平面参数；

标志物的三维信息获取子单元，其用于基于地面假设模型恢复地平面参数拟合子单元获得的近地特征点的真实空间坐标，进而得到特征点三维信息获取子单元获得的所有无尺度特征点的真实尺度，依此获取各标志物的三维信息。

进一步地，所述拓扑构建单元具体包括：

标志物距离获取子单元，其用于根据γ_l＝γ_d÷d，判断当前帧所关联标志物位置可信度γ_l，其中，γ_d表示根据YOLO-v5检测时网络直接输出的标志物检测置信度，d表示由标志物二维信息获取单元获得的各标志物的三维信息获取的标志物到当前帧之间的距离；

标志物搜索子单元，其用于保留当前帧所关联标志物的γ_l超过阈值γ_threshold的标志物，其余标志物进行时域跟踪，直到该标志物满足γ_l>γ_threshold，期间，若该标志物连续三帧丢失，则丢弃该标志物；

拓扑连接图构建子单元，其用于当前帧所检测到的可信标志物数量超过预设数时，连接车辆位姿节点及标志物节点，构建二维拓扑图，并依据位置可信度赋予连接边权重，构建标志物拓扑连接图。

本发明由于采取以上技术方案，其具有以下优点：

1、本发明采取双定位线程并行模式，视觉SLAM定位线程在短时域内误差较小，为离线语义地图匹配定位线程提供地图匹配初始位置，可有效避免后者匹配定位时的发散问题，且当离线语义地图匹配定位线程无法检测到语义信息时仍可获得较为准确的位置信息；

2、所述视觉SLAM定位线程结合地面模型，采取滑动窗口策略跟踪多帧近地特征点，恢复地面参数，有效抑制单目视觉SLAM过程所产生的尺度漂移；

3、所述离线语义地图匹配定位线程反馈全局位置信息，可消除视觉SLAM定位线程累计漂移；

4、众包式地图数据库在线更新方法有利于提升地图信息时效性。

附图说明

图1为本发明实施例提供的自动驾驶车辆城市结构化场景纯视觉定位方法的架构图。

图2为本发明实施例提供的离线语义地图加载及更新流程示意图。

具体实施方式

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

如图1和图2所示，本发明实施例提供的自动驾驶车辆城市结构化场景纯视觉定位方法包括：

设置并行的两线程：

一所述线程为视觉SLAM定位线程，其用于采用基于局部地图的ORB(OrientedFAST and Rotated BRIEF，具有旋转-尺度不变性的快速提取特征点)特征点法进行定位，实时获取车辆当前位姿，车辆位姿包括车辆的位置(x，y，z)、姿态(横摆角、侧偏角、俯仰角)；

另一所述线程为离线语义地图匹配定位线程，其用于通过地面先验，恢复视觉SLAM线程所获取的特征点的尺度，其具体包括：

1)训练并使用YOLO-v5目标检测算法检测相机获取的原始图像中的结构化语义标志物，包含但不限于车道线、道路标志牌和交通信号灯等不同类型的目标。

2)根据地面先验，例如：相机相对于地面的安装高度和角度，恢复步骤1)检测到的标志物在相机坐标系中的三维信息，再投影获取标志物在图像坐标系的二维信息。

3)对步骤1)检测到的标志物进行时域跟踪，抑制同一标志物在连续帧检测过程中的漏检、误检。当同一帧检测获取标志物数量不少于预设数目时，构建车辆与标志物的二维拓扑图，其拓扑边权重由当前帧所关联的标志物可信度确定。

4)根据车辆当前位置，依据步骤3)构建好的车辆与标志物的二维拓扑图进行地图搜索匹配，并恢复车辆当前二维位姿。

5)由时序位姿信息构建运行轨迹，将运行轨迹的信息和所述标志物信息上传至云端，更新离线地图语义信息库。

本发明实施例仅使用相机，检测环境中的结构化语义信息，并通过相机安装先验恢复出标志物的几何尺寸，将标志物与离线地图数据库进行匹配、优化求解车辆当前二维位置信息，为维护实时性，对离线地图数据库信息进行在线更新。

上述步骤1)中，针对不同标志物特性的差异性，采取双模型独立检测策略对所述标志物进行检测，具体包括：

1.1)对于三维物体，例如道路标志牌、交通信号灯等物体，直接在去畸变后的原始图像中进行目标检测。

1.2)对于地面上的二维目标，例如车道线、交通引导线等，对原始图像进行鸟瞰图投影，获得单帧鸟瞰图，然后将不同时刻的鸟瞰图进行拼接，最后在拼接好的鸟瞰图中进行目标检测。

还可以使用原图同步或现有技术其它方法检测1.1)、1.2)所述二维、三维目标。

上述步骤2)中，根据地面先验，例如：相机相对于地面的安装高度和角度，恢复步骤1)检测到的标志物在相机坐标系中的三维位置的具体方法包括：

2.1)在同步运行的视觉SLAM定位线程中，提取ORB特征点，再采取三角化不断恢复特征点三维信息，此时特征点无尺度约束。

其中，提取ORB特征点的方法具体包括：在标志物识别框中采取更低的自适应阈值进行特征点提取，以保证每个标志物识别框中特征点的数量大于预设数，例如：10。

2.2)在每个标志物识别框内选择所需的标志物。

步骤2.2)具体包括：

2.21)计算每个标志物识别框内所述三维特征点到相机中心距离的均值

及标准差σ_d；

2.22)剔除满足如下条件之一的所述三维特征点：

a.所述三维特征点到相机中心距离大于所述均值

预设倍数的远点；

b.所述三维特征点到相机中心距离与所述均值

的差值最大的点；

2.23)返回步骤2.21)，直至特征点的距离收敛，即：识别框内的所述三维特征点的标准差σ_d小于预设距离值(例如：0.5米)最后丢弃特征点的数量小于预设数的标志物，比如：5。

其中，收敛判别准则为：所计算标准差小于经验值，比如0.5米。

2.3)将步骤2.1)得到的三维特征点反投影到相机帧中，估计相关地平面参数并利用几何约束细化选取近地特征点，具体过程如下：

2.31)可以采用Hesse形式参数化表示地平面，如：平面单位法向量

和平面距离远点的距离d。还可以选取球坐标形式、切平面形式等表示地平面参数化形式。

2.32)构建平面匹配模型

使用RANSAC(Random SampleConsensus,随机采样一致性)算法拟合地平面；其中，

表示平面单位法向量，n_π1,n_π2,n_π3分别为单位法向量

在三轴的分量，X_i表示第i个所述三维特征点的三维空间坐标，d表示平面距离坐标系原点的距离。

2.33)计算步骤2.1)得到的三维特征点X_i到平面距离

判断d_i是否小于距离阈值d_threshold，如果是，则判定该三维特征点为近地特征点。其中，d_threshold为经验值，例如：0.05米。

2.4)使用滑动窗口策略跟踪连续多帧近地特征点，依照步骤2.3)选取的近地特征点策略再次获取最优地面点集并拟合地平面参数，此时距离阈值

可以选取为d_threshold的一半，即

其中，滑动窗口策略中的滑动窗口尺寸n_w为经验值。当然，该步骤也可以构建局部地图选取近地特征点，即保留当前帧、以及与当前帧具有共视特征点的帧的所有近地特征点。获取最优地面点集并拟合地平面参数的方法可以使用RANSAC平面拟合法，也可以使用PCA(Principal Component Analysis，主成分分析)平面拟合法，还可以使用平面聚类算法等。

2.5)基于地面假设模型恢复2.4)获得的近地特征点的真实空间坐标，进而得到步骤2.1)获得的所有无尺度特征点的真实尺度，依此获取各标志物的三维信息。

上述步骤3)中，构建车辆与标志物的二维拓扑图，以清晰表达标志物同当前时刻车辆位姿的连接关系，其方法包括：

3.1)根据标志物检测置信度和标志物距离，判断当前帧所关联标志物位置可信度，即γ_l＝γ_d÷d，其中，γ_l表示标志物位置可信度，γ_d表示根据YOLO-v5检测时网络直接输出的标志物检测置信度，d表示由步骤2.5)获得的各标志物的三维信息获取的标志物到当前帧之间的距离。

3.2)保留当前帧所关联标志物位置可信度超过阈值γ_threshold的标志物，其余标志物进行时域跟踪，即后续连续时刻重复上述步骤3.1)到3.2)过程，直到该标志物满足γ_l>γ_threshold，期间，若该标志物连续三帧丢失，则丢弃该标志物。其中，γ_threshold的具体数值值由经验值给定，例如0.025m^-1。

3.3)当前帧所检测到的可信标志物数量超过预设数时，连接车辆位姿节点及标志物节点，构建二维拓扑图，并依据位置可信度赋予连接边权重，构建标志物拓扑连接图。其中，预设数选择求解车辆位姿最少需要的数目，如：2个。

上述步骤4)中，为满足增量更新需求，选取NDS(Navigation Data StandardAssociation，面向汽车生态系统的地图数据全球标准)地图数据存储标准，地图主要由路网、标志物POI、名称和视图四部分组成，在网络良好时自动从云端将地图下载到车端。步骤4)具体包括：

4.1)使用下式提供的匀速车辆模型获取当前时刻i车辆初始位姿T_i：

T_i＝T_i-1 ²/T_i-2

式中，T_i-2、T_i-1分别表示i-2、i-1时刻的车辆位姿。

对于多传感器而言，还可以使用IMU数据、轮速计数据等。

4.2)根据步骤4.1)所获取当前时刻车辆位姿T_i、步骤1)标志物检测所获取当前所观测到标志物类别，在预先下载好的NDS离线地图的相应层级中搜索同类别标志物信息及周围路网信息，为保证信息加载轻量性及搜索可靠性，加载当前时刻初始位置T_i周围九宫格的地图块信息，该信息于车端(车上的计算机)分配相应空间进行缓存。

4.3)依据步骤4.2)中的当前所观测到的标志物类别、当前时刻初始位置T_i进行搜索匹配，依据下式计算得到的地图特征置信度α_s：

α_s＝γ_l×σ

式中，σ表示该类别物体在周边密集程度，该参数反映标志物误匹配概率。

也可以不考虑该类别标志物的密集程度，即α_s＝γ_l。

4.4)选择置信度最高的两个标志物位置信息，恢复车辆当前二维位姿，返回步骤4.3)，直到定位结果收敛。其中，恢复车辆当前二维位姿的方法包括：

使用当前帧所选取的两个标志物相对于当前相机位姿的观测误差构建当前帧误差方程e＝[e₁ e₂]，使用奇异值分解求解e＝0，得到车辆当前二维位姿，其中，e₁和e₂通过以下误差方程获得：

式中，m取1、2分别表示当前帧所关联的两个标志物序号，

表示标志物m相对于当前相机位姿的径长观测误差，

表示标志物m相对于当前相机位姿的角度观测误差；w_m＝[x^m y^m]^τ表示标志物m在当前时刻相机坐标系下的真实位置；

表示标志物m的观测值，

分别表示标志物m的径长和角度的观测值；

表示观测方程；τ表示转置；上式由观测方程与观测值作差获得，求解得到当前帧的车辆二维位姿w_i。

上述实施例中，还可以采用BA优化或卡尔曼滤波等方法恢复车辆当前二维位姿。

上述步骤5)为离线过程，其包含步骤如下：

5.1)车端将车辆轨迹信息、标志物待更新信息同步传输给云端；

5.2)云端同步加载离线地图数据和待更新地图数据；

5.3)基于车辆轨迹信息拓扑关系与离线地图道路网建立关联关系；

5.4)建立离线地图数据和待更新地图数据之间的标志物关联关系；

5.5)收集关联地物空间位置点及关联车道特征变化点坐标，根据所述特征变化点构建仿射变换关系，通过已生成仿射变换关系对待更新数据中的车道、地物空间坐标进行仿射变换，生成变换后车道数据和地物数据，提升待更新数据精度；

5.6)对比待更新数据与离线地图数据，识别提取车道级差异数据，利用所述差异数据，更新地图数据。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种自动驾驶车辆城市结构化场景纯视觉定位方法，其特征在于，包括：

设置并行的两线程：

一所述线程为视觉SLAM定位线程，其用于采用基于局部地图的ORB特征点法实时获取车辆当前位姿；

另一所述线程为离线语义地图匹配定位线程，其用于通过地面先验，恢复所述视觉SLAM定位线程所获取的特征点的尺度，其具体包括：

1)训练并检测相机获取的原始图像中的结构化语义标志物；

2)根据地面先验，获取所述标志物在图像坐标系的二维信息；

3)对所述标志物进行时域跟踪，当同一帧检测获取所述标志物数量不少于预设数目时，构建车辆与标志物的二维拓扑图，且其拓扑边权重由当前帧所关联的所述标志物可信度确定；

4)根据车辆当前位置，依据所述二维拓扑图进行地图搜索匹配，以恢复车辆当前二维位姿；

5)由连续时刻车辆二维位姿信息构建运行轨迹，将所述运行轨迹的信息和所述标志物信息上传至云端，更新离线地图语义信息库。

2.如权利要求1所述的自动驾驶车辆城市结构化场景纯视觉定位方法，其特征在于，所述步骤2)具体包括：

2.1)在同步运行的视觉SLAM定位线程中，提取ORB特征点，再采取三角化不断恢复无尺度约束的三维特征点的三维信息；

2.2)在每个标志物识别框内选择所需的标志物；

2.3)将步骤2.1)得到的三维特征点反投影到相机帧中，估计相关地平面参数并利用几何约束细化选取近地特征点；

2.4)使用滑动窗口策略跟踪连续多帧近地特征点，依照步骤2.3)选取的近地特征点策略再次使用RANSAC平面拟合法，获取最优地面点集并拟合地平面参数；

2.5)基于地面假设模型恢复2.4)获得的近地特征点的真实空间坐标，进而得到步骤2.1)获得的所有无尺度特征点的真实尺度，依此获取各标志物的三维信息。

3.如权利要求2所述的自动驾驶车辆城市结构化场景纯视觉定位方法，其特征在于，所述步骤2.2)具体包括：

2.21)计算每个标志物识别框内所述三维特征点到相机中心距离的均值d及标准差σ_d；

2.22)剔除满足如下条件之一的所述三维特征点：

a.所述三维特征点到相机中心距离大于所述均值

预设倍数的远点；

b.所述三维特征点到相机中心距离与所述均值

的差值最大的点；

2.23)返回步骤2.21)，直至识别框内的所述三维特征点的标准差σ_d小于预设距离值，最后丢弃三维特征点的数量小于预设数的标志物。

4.如权利要求2所述的自动驾驶车辆城市结构化场景纯视觉定位方法，其特征在于，所述步骤2.3)具体包括：

2.31)采用Hesse形式参数化表示地平面；

2.32)构建平面匹配模型

使用RANSAC算法拟合地平面；其中，

表示平面单位法向量，n_π1,n_π2,n_π3分别为单位法向量

在三轴的分量，X_i表示第i个所述三维特征点的三维空间坐标，d表示平面距离坐标系原点的距离；

2.33)计算步骤2.1)得到的三维特征点X_i到平面距离

判断d_i是否小于距离阈值d_threshold，如果是，则判定该三维特征点为近地特征点。

5.如权利要求1-4中任一项所述的自动驾驶车辆城市结构化场景纯视觉定位方法，其特征在于，所述步骤3)具体包括：

3.1)根据γ_l＝γ_d÷d，判断当前帧所关联标志物位置可信度γ_l，其中，γ_d表示根据YOLO-v5检测时网络直接输出的标志物检测置信度，d表示由步骤2)获得的各标志物的三维信息获取的标志物到当前帧之间的距离标志物距离；

3.2)保留当前帧所关联标志物的γ_l超过阈值γ_threshold的标志物，其余标志物进行时域跟踪，即后续连续时刻重复上述步骤3.1)到3.2)，直到该标志物满足γ_l>γ_threshold，期间，若该标志物连续三帧丢失，则丢弃该标志物；

3.3)当前帧所检测到的可信标志物数量超过预设数时，连接车辆位姿节点及标志物节点，构建二维拓扑图，并依据位置可信度赋予连接边权重，构建标志物拓扑连接图。

6.如权利要求5所述的自动驾驶车辆城市结构化场景纯视觉定位方法，其特征在于，所述步骤4)具体包括：

4.1)使用下式提供的匀速车辆模型获取当前时刻i车辆初始位姿T_i：

T_i＝T_i-1 ²/T_i-2

式中，T_i-2、T_i-1分别表示i-2、i-1时刻的车辆位姿；

4.2)根据步骤4.1)所获取当前时刻车辆位姿T_i、步骤1)标志物检测所获取当前所观测到标志物类别，在预先下载好的NDS离线地图的相应层级中搜索同类别标志物信息及周围路网信息，并加载当前时刻初始位置T_i周围九宫格的地图块信息，并将该地图块信息于车端分配相应空间进行缓存；

4.3)依据步骤4.2)中的当前所观测到的标志物类别、当前时刻初始位置T_i进行搜索匹配，依据γ_l计算得到的地图特征置信度α_s；

4.4)选择置信度最高的两个标志物位置信息，恢复车辆当前二维位姿，返回步骤4.3)，直到定位结果收敛。

7.如权利要求6所述的自动驾驶车辆城市结构化场景纯视觉定位方法，其特征在于，所述步骤4.4)中，恢复车辆当前二维位姿的方法具体包括：

使用当前帧所选取的两个标志物相对于当前相机位姿的观测误差构建当前帧误差方程e＝[e₁ e₂]，使用奇异值分解求解e＝0，得到车辆当前二维位姿，其中，e₁和e₂通过以下误差方程获得：

式中，m取1、2分别表示当前帧所关联的两个标志物序号，

表示标志物m相对于当前相机位姿的径长观测误差，

表示标志物m相对于当前相机位姿的角度观测误差；w_m＝[x^m y^m]^τ表示标志物m在当前时刻相机坐标系下的真实位置；

表示标志物m的观测值，

分别表示标志物m的径长和角度的观测值；

表示观测方程；τ表示转置；上式由观测方程与观测值作差获得，求解得到当前帧的车辆二维位姿w_i。

8.一种自动驾驶车辆城市结构化场景纯视觉定位系统，其特征在于，包括：

视觉SLAM定位线程，其用于采用基于局部地图的ORB特征点法实时获取车辆当前位姿；

离线语义地图匹配定位线程，其用于通过地面先验，恢复所述视觉SLAM定位线程所获取的特征点的尺度，其具体包括：

标志物检测单元，其用于训练并检测相机获取的原始图像中的结构化语义标志物；

标志物二维信息获取单元，其用于根据地面先验，获取所述标志物在图像坐标系的二维信息；

拓扑构建单元，其用于对所述标志物进行时域跟踪，当同一帧检测获取标志物数量不少于预设数目时，构建车辆与标志物的二维拓扑图，且其拓扑边权重由当前帧所关联的标志物可信度确定；

位姿恢复单元，其用于根据车辆当前位置，依据所述二维拓扑图进行地图搜索匹配，以恢复车辆当前二维位姿；

信息更新单元，其用于由时序位姿信息构建运行轨迹，将运行轨迹的信息和所述标志物信息上传至云端，更新离线地图语义信息库。

9.如权利要求8所述的自动驾驶车辆城市结构化场景纯视觉定位系统，其特征在于，所述标志物二维信息获取单元具体包括：

特征点三维信息获取子单元，其用于在同步运行的视觉SLAM定位线程中，提取ORB特征点，再采取三角化不断恢复特征点三维信息，此时的特征点无尺度约束；

标志物选取子单元，其用于在每个标志物识别框内选择所需的标志物；

近地特征点选取子单元，其用于将特征点三维信息获取子单元得到的三维特征点反投影到相机帧中，估计相关地平面参数并利用几何约束细化选取近地特征点；

地平面参数拟合子单元，其用于使用滑动窗口策略跟踪连续多帧近地特征点，依照近地特征点选取子单元选取的近地特征点策略再次使用RANSAC平面拟合法，获取最优地面点集并拟合地平面参数；

标志物的三维信息获取子单元，其用于基于地面假设模型恢复地平面参数拟合子单元获得的近地特征点的真实空间坐标，进而得到特征点三维信息获取子单元获得的所有无尺度特征点的真实尺度，依此获取各标志物的三维信息。

10.如权利要求8或9所述的自动驾驶车辆城市结构化场景纯视觉定位系统，所述拓扑构建单元具体包括：

标志物距离获取子单元，其用于根据γ_l＝γ_d÷d，判断当前帧所关联标志物位置可信度γ_l，其中，γ_d表示根据YOLO-v5检测时网络直接输出的标志物检测置信度，d表示由标志物二维信息获取单元获得的各标志物的三维信息获取的标志物到当前帧之间的距离；

标志物搜索子单元，其用于保留当前帧所关联标志物的γ_l超过阈值γ_threshold的标志物，其余标志物进行时域跟踪，直到该标志物满足γ_l>γ_threshold，期间，若该标志物连续三帧丢失，则丢弃该标志物；

拓扑连接图构建子单元，其用于当前帧所检测到的可信标志物数量超过预设数时，连接车辆位姿节点及标志物节点，构建二维拓扑图，并依据位置可信度赋予连接边权重，构建标志物拓扑连接图。

Images