CN115717894A - 一种基于gps和普通导航地图的车辆高精度定位方法 - Google Patents

Abstract

本发明公开了一种基于GPS和普通导航地图的车辆高精度定位方法，包括以下步骤：利用车载GPS模块确定地图初始定位节点；利用车载摄像机和普通导航地图获取道路数据；识别和提取道路关键目标；关联关键目标数据；计算车辆位姿；基于车道线检测消除横向位置误差。本发明以普通级导航地图为引导，将先验地图知识与车端环视感知结果相融合，基于车道线、路标、商户等交通要素感知结果和数据关联的结果精确计算车辆的位姿结果，并基于节点匹配的方法进一步消除横向定位误差，使自动驾驶车辆在行驶过程基于额外的道路信息和约束信息将定位精度提升至车道级，既提高了智能车辆在城市道路定位的精度和鲁棒性又提高了道路多层级数据融合定位的实时性。

Description

一种基于GPS和普通导航地图的车辆高精度定位方法

技术领域

本发明车辆定位技术领域，尤其涉及一种基于GPS和普通导航地图的车辆高精度定位方法。

背景技术

获取准确的车辆高精度定位信息和运动状态信息是无人驾驶技术核心问题之一，是提升自动驾驶安全性的关键技术。自动驾驶系统融合定位系统可实现车辆行驶轨迹记录、高精地图绘制、车辆实时状态检测等智能车辆的自动驾驶需求。目前无人驾驶车辆通常使用GPS系统来获取实时位置，但民用GPS模块的定位精度普遍为10米级，无法满足无人驾驶在决策、规划等领域的定位需求。基于差分原理的RTK方法可以获取实时厘米级的定位精度，但是需要基准站和流动站同时作业，定位的精度与基站数量有关，因此该方法的成本较高，作业的距离有限。

发明内容

为解决现有技术存在的上述问题，本发明要设计一种既能提高智能车辆在城市道路定位的精度和鲁棒性又能提高道路多层级数据融合定位的实时性的基于GPS和普通导航地图的车辆高精度定位方法。

为了实现上述目的，本发明的技术方案如下：一种基于GPS和普通导航地图的车辆高精度定位方法，利用车载摄像机、惯性测量单元、车载GPS模块和车规级工控机进行定位，具体方法包括以下步骤：

A、利用车载GPS模块确定地图的初始定位节点

A1、利用车载GPS模块获取车辆的实时原始数据；所述原始数据包括经纬度、海拔高度和航向角，随后由车规级工控机的计算单元对原始数据进行解码并判断GPS模块状态。在GPS模块正常采集数据后，使用递推平均值数字滤波算法对解码后的原始数据进行滤波处理，去除因解码后的原始数据中噪声引起的误差，使得解码后的原始数据更加平滑。递推平均滤波公式如下：

其中，x表示滤波后的GPS数据，x_i表示第i条解码后的原始数据，N表示序列长度。

A2、根据滤波后的GPS数据进行坐标转换，获取车辆初始位置的三维坐标，并计算与先验地图节点的欧式距离，选取最近的道路节点作为初始的定位节点。在得到滤波后的经纬度、海拔高度和航向角后，获得车辆的初始位置，并计算与先验地图中节点的欧氏距离，选取距离最小的节点作为匹配定位结果节点。初始定位位置和地图节点的欧氏距离计算公式如下：

其中，ρ为欧氏距离，

分别为先验地图中第i个节点P_i的X轴、Y轴、Z轴的坐标值，x₂,y₂,z₂分别为车辆初始定位位置的X轴、Y轴、Z轴的坐标值。

选取车辆GPS初始定位位置与先验地图中欧氏距离最小的节点，将该节点作为初始定位节点P_init，并将该节点的三维坐标作为车辆在世界坐标系下的初始坐标。

B、利用车载摄像机和普通导航地图获取道路数据

对车载摄像机进行标定，并采集车辆周围的街景图像，将采集到的数据存储在车规级工控机的计算单元开辟的vector格式数据库中。所述车载摄像机为环视车载摄像机。

B1、对车载摄像机进行标定，获取车载摄像机的内参矩阵、外参矩阵、畸变系数，并使用针孔相机模型校正摄像机得到的图像。使用内部参数获取车载摄像机从三维空间到二维图像的投影关系，使用外部参数获取车载摄像机坐标与世界坐标系之间相对位置关系。以车辆的车头方向为Y轴正向，横向向右的方向为X轴正向，垂直水平面朝上的方向为Z轴正向，假设车载摄像机与大地的坐标系变换公式为：

其中，α为车载摄像机坐标系绕X轴旋转的角度，β为车载摄像机坐标系绕Y轴旋转的角度，γ为车载摄像机坐标系绕Z轴旋转的角度，x,y,z分别为车载摄像机坐标系下一点p₀的三维坐标，x',y',z'分别为点p₀对应世界坐标系下的点p'₀的三维坐标。

B2、道路数据具体的采集储存过程为：车辆在道路上以固定的车速进行行驶，车载摄像机以固定采样频率采集行驶方向和垂直行驶方向的视觉场景图像，视觉场景图像的分辨率为1280*720。随后，将采集的视觉场景图像进行压缩，并将压缩后的视觉场景图像存储在数据库中。

C、识别和提取道路关键目标

C1、将车载摄像机拍摄的视觉场景图像数据和普通导航地图视觉场景图像输入已经完成训练的轻量化目标检测模型，对关键目标进行识别和提取，所述关键目标包括交通标志、广告牌和文字标牌；所述交通标志包括禁令标志、警告标志、指示标志、指路标志、旅游区标志、道路施工安全标志和辅助标志。

所述轻量化目标检测模型为在YOLO5s网络模型的基础上添加多尺度通道注意力模块和注意力特征融合模块。轻量化目标检测模型的损失函数分为位置损失函数L_CIoU、置信度损失函数L_obj和分类损失函数L_cls，总损失函数为三部分的和。位置损失函数采用CIoU损失函数，置信度损失函数和分类损失函数均采用二分类的交叉熵损失函数。

C2、轻量化目标检测模型分别对车载摄像机采集图像和普通导航街景图像中的关键目标进行检测，通过检测结果的标签文件获取目标区域像素坐标值，并为所述关键目标设置单独的索引号以便后期的查询。关键目标的图像像素坐标和索引号组成目标关键信息。根据检测的目标类别，将交通标志类型的目标关键信息、所属视觉场景图像和文字标牌的目标关键信息分别保存在数据库中。

D、关联关键目标数据

D1、提取普通导航地图中固定半径范围内的所有目标关键信息，并将车载摄像机拍摄的视觉场景图像中提取的关键目标依次与其进行视觉特征提取和匹配。

根据GPS模块确定的初始定位位置的结果，获取普通导航地图中以R_s为半径的所有关键目标信息，将其设为待匹配的关键目标。将车载摄像机拍摄的视觉场景图像通过轻量化目标检测模型提取的关键目标依次与普通导航街景地图提取的关键目标进行视觉特征提取和匹配，具体方法如下：

通过Hesse矩阵的判别式来计算特征值，使用高斯差分算法进行计算。在特征提取后采用特征向量之间的相似性来进行特征匹配，使用欧式距离作为判断特征向量相似性的指标，表示如下：

其中，d_ii为特征点u_i和v_i的欧氏距离，

为特征点u_i和v_i的特征向量。

D2、利用RANSAC算法对匹配的点对进行筛选，筛选后的特征点数目大于阈值则判定待匹配的关键目标匹配成功，记录该关键目标的信息。具体方法如下：

利用RANSAC算法进行筛选，找出匹配的内点和偏离模型正常范围的外点。随机从特征点对匹配集中选取4对匹配点计算相对变换的矩阵来判断初始匹配集中哪些特征点对匹配符合条件，迭代n次后选取内点最多的结果作为最后的匹配结果；利用RANSAC算法剔除错误的特征点对，记录最终剩余特征点对的个数N_t。设置匹配成功的阈值为N_i。如果关键目标中正确匹配的特征点对N_r大于阈值N_i则判定该关键目标匹配成功，否则匹配失败继续匹配新的关键目标。

D3、基于车载摄像机拍摄的视觉场景图像采集的文字特征进行扫描识别，将结果与固定半径范围内的普通导航地图中文字识别结果进行匹配，获取关联的数据。

匹配成功后分别获取在普通导航街景地图中关键目标拍摄点的三维坐标和该目标所属的场景图像，以及在车载摄像机中关键目标所属的视觉场景图像。将含有广告牌、文字标牌的关键目标进行文字识别，并与普通导航地图所提供的信息进行文字扫描匹配。根据扫描匹配的结果，记录车载摄像机和普通导航地图的目标内部文字一致的广告牌、文字标牌的关键目标的索引，从而获取关联数据。

E、计算车辆位姿

通过普通导航地图中关键目标像素位置和拍摄点的三维位置信息进行坐标转换，并结合基于文字扫描匹配结果获取关键目标对应的真实世界坐标。

基于导航地图提供关键目标位置信息，利用匹配成功的关联数据对当前车辆的位姿进行计算。

通过普通导航地图街景图像目标像素位置和拍摄点的三维世界坐标进行转换，获取关键目标对应的真实世界坐标。设pix₁、pix₂分别为两个特征点的归一化坐标，则满足下式：

s₂pix₂＝s₁Rpix₁+t

由于测量点车载摄像机的位置信息由街景地图获取，R和t为根据当前帧车载摄像机到前一帧车载摄像机坐标系间的外参计算得到，s₁、s₂是待求解参数。通过化简得到下式：

其中左侧为零，使用最小二乘法求解s₂，进而得到s₁，获得两个特征点对应像素坐标三角化后的三维信息。

根据车载图像与关键目标对应的真实世界坐标和在图像中的像素坐标计算当前状态的车辆位姿，具体方法如下：

根据车载摄像机拍摄的图像与关键目标对应的真实世界坐标计算正在行驶的车辆相对位姿。利用车辆初始位置和相对位姿变换计算车辆当前时刻的三维位置。利用上述位姿计算得到的三维位置，计算其与先验地图中节点的欧氏距离，选取距离最小的节点作为匹配定位结果节点。

F、基于车道线检测消除横向位置误差

F1、对视觉场景图像进行增强处理，使用Sobel算子计算图像梯度，利用非极大值抑制来去除非边缘的噪点并细化边缘，使用可变阈值的方法进行分割并将边缘和背景区分，具体方法如下：

对道路图像进行前处理，以增强道路的车道线部分。首先，将图像转变为灰度图像，使用高斯滤波器进行平滑。然后，使用基于图像梯度的Canny算法进行图像的边缘提取。使用Sobel算子计算图像梯度，使用可变阈值的方法进行分割并将边缘和背景区分，采用整体图像的灰度值为高阈值，低阈值根据高阈值进行变换。

F2、使用Hough变换检测图像中的车道线，对检测出的多条车道线基于K-means聚类算法进行聚类，获取最终的车道线检测结果，具体方法如下：

使用Hough变换检测经过二值化后图像中的车道线，将图像直线映射到参数空间，遍历图像的非零像素点进行参数投票，找出参数空间大于阈值的参数点。由于车道线会检测出多条直线，采用K-Means算法进行聚类从而获取最终的车道线检测结果。

F3、根据车道线检测结果和车辆的边缘信息来推断车辆所属的车道，匹配出最优的道路节点，提高车辆的定位精度。

借助车辆的边缘信息在图像上划定车辆的前进区域和中心线，然后对出现在中心线两侧的车道线进行计数。若存在车道线在车辆的前进区域内部，则推断车辆在车道中心节点，否则推断车辆在车道线节点。

借助车道线识别的结果，将车辆所在车道上固定半径范围的地图节点视为待匹配的节点，然后逐一计算当前车辆三维坐标和所有待匹配节点三维坐标的欧氏距离。选取欧式距离最小的地图节点为匹配的最优道路节点，并将该节点的三维坐标位置视为车辆优化后的三维坐标。

与现有技术相比，本发明具有以下有益效果：

普通导航地图中能够提供广泛的街景数据和道路文字信息，基本实现了城市道路视野全覆盖，这些数据和信息包括交通标志、广告牌、公交站牌、文字标牌等，这些信息构成一个城市的三维位置数据库，为智能车辆的高精度定位提供额外的约束信息。本发明以普通级导航地图为引导，将先验地图知识与车端环视感知结果相融合，基于车道线、路标、商户等交通要素感知结果和数据关联的结果精确计算车辆的位姿结果，并基于节点匹配的方法进一步消除横向定位误差，使自动驾驶车辆在行驶过程基于额外的道路信息和约束信息将定位精度提升至车道级，既提高了智能车辆在城市道路定位的精度和鲁棒性又能提高道路多层级数据融合定位的实时性。

附图说明

图1是本发明的定位系统流程示意图。

图2是本发明的横向误差消除方法流程示意图。

图3是本发明的定位节点匹配示意图。

具体实施方式

为了让本发明的目的、技术路线以及技术优势更加清晰，以下结合附图及实施例，对本发明作进一步详细阐述。显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。应当理解，本领域的技术人员可根据本说明书所描述的内容了解本发明的特点，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，为了避免混乱或模糊本发明的重点，有些具体技术细节将在描述过程中被省略。

一种基于GPS和普通导航地图的车辆高精度定位方法，用于智能车辆基于感知结果和数据关联并进行精确的车辆定位，包括GPS初始定位、道路数据模块、目标检测识别、特征匹配和文字识别、车辆位姿计算、横向误差消除。如图1所示，本发明的流程如下：

步骤一、利用车载GPS模块获取初始定位节点

车载GPS模块用来获取车辆的实时经纬度信息、海拔信息、航向角信息等原始数据。随后基于串口编程和GGA协议对原始数据进行读取和解析，并判断GPS状态。在GPS正常采集数据后，使用递推平均值数字滤波算法对观测值进行滤波处理，将连续一段的数据看成一个序列，将GPS最新采集的新数据放在序列后端，舍弃序列前端旧的数据，将整个序列的数据进行算术平均值，获得的均值作为滤波的结果。

在得到滤波后的经纬度、海拔信息和航向角信息等数据后，基于发布器完成消息的发布，其中串口设置为/dev/ttyUSB0，波特率设置为115200，发送频率为50hz。基于发布的数据获得车辆三维坐标位置和车辆的朝向，并根据车辆三维位置计算与先验地图中节点的欧氏距离，选取距离最小的节点作为匹配定位结果节点。设置欧氏距离的阈值为10m，即当前GPS显示的车辆位置与最近地图节点的欧式距离超过10m视为GPS定位失败，读取下一帧数据重新计算。

选取车辆GPS初始定位位置与先验地图欧氏距离最小的节点为初始定位节点，该节点对应的世界坐标系下的三维坐标为车辆的初始坐标。

步骤二、对车载摄像机进行标定，并采集车辆周围的街景图像，将采集到的数据存储在数据库中。

对车载摄像机进行标定，得到摄像机的内外参矩阵、畸变系数，并使用针孔相机模型校正摄像机得到的图像。标定内参时，使用摄像机和黑白棋盘格标定板拍摄10张图像，在Visual Studio内使用OpenCV进行标定，图像的分辨率为1280*720，棋盘格的大小为16mm，并将标定误差大于0.1像素的图像删除提高精度。内参标定获取摄像机的水平焦距、垂直焦距、水平方向中心点、垂直方向中心点。在摄像机内参已知情况下，利用PnP算法完成对外参(位姿)的求解。在摄像机视野范围内场地布置标记点时，标记点的个数应大于6个点，并将重投影误差大于1像素的点进行剔除，外参标定获取摄像机和世界坐标系之间的旋转矩阵和平移矩阵。

标定完成后，进行实车采集数据，数据具体的采集储存过程为：车辆在道路上以20km/h固定的车速进行行驶，车载摄像机以固定的20Hz采样频率采集行驶方向和垂直行驶方向的视觉场景图像，图像的分辨率为1280*720。随后，将采集的视觉场景图像进行压缩以节约储存空间、提高目标检测算法的效率，随后将街景图像存储在数据库中。

步骤三、道路关键目标的识别和提取

将车载摄像机拍摄的视觉场景图像数据和普通导航地图视觉场景图像输入到已经完成训练的轻量化目标检测模型。目标检测模型的损失函数为

L＝ω₁L_CIoU+ω₂L_obj+ω₃L_cls

其中，L为总的损失，L_CIoU为位置损失函数，L_obj为置信度损失函数，L_cls是分类损失函数，ω₁、ω₂、ω₃为不同损失函数的权重。

目标检测模块分别对车载摄像机采集图像和普通导航地图街景图像中的交通标志、广告牌、文字标牌等进行检测，通过检测结果的标签文件获取目标区域像素坐标值，并为该目标设置单独的索引号以便后期的查询。关键目标的图像像素坐标和索引号组成目标关键信息。其中，提取的交通标志包括禁令标志、警告标志、指示标志、指路标志、旅游区标志、道路施工安全标志和辅助标志。根据检测的目标类别，将交通标志类型的目标关键信息、所属街景图像和文字标牌的目标关键信息、所属街景图像分别保存在数据库中。

步骤四、交通标志牌匹配、街边商家名称文字识别匹配

根据GPS初始定位的结果，获取普通导航地图中以R_s为半径的所有目标关键信息，将其设为待匹配的目标。将车载图像通过目标检测网络提取的关键目标依次与普通导航街景地图提取的关键目标进行视觉特征提取和匹配。

通过Hesse矩阵的判别式来计算特征值，使用高斯差分算法进行计算。在特征提取后采用特征向量之间的相似性来进行特征匹配，使用欧式距离作为判断特征向量相似性的指标，可以表示为

其中，d_ii为特征点u和v_i的欧氏距离，

为特征点u_i和v_i的特征向量。

利用RANSAC算法进行筛选，找出匹配的内点和偏离模型正常范围的外点。随机从特征点对匹配集中选取4对匹配点计算相对变换的矩阵来判断初始匹配集中那些匹配符合条件，迭代n次后选取内点最多的结果作为最后的匹配结果利用RANSAC算法剔除错误的匹配点，记录最终剩余匹配点对的个数N_r。设置匹配成功的阈值为N_i。如果关键目标中正确匹配的特征点对N_r大于阈值N_i则判定该关键目标匹配成功，否则匹配失败继续匹配新的关键目标。匹配成功后分别获取在普通导航街景地图中关键目标拍摄点的三维坐标和该目标所属的街景图像，以及在车载摄像机中关键目标所属的图像。将含有道路限速、公交站牌、商户店名等信息的关键目标进行文字识别，并与普通导航地图所提供的信息进行文字扫描匹配。

步骤五、基于导航地图提供关键目标位置信息，利用匹配成功的关联数据对当前车辆的位姿进行计算。

通过普通导航地图街景图像目标像素位置和拍摄点的三维世界坐标进行转换，获取关键目标对应的真实世界坐标。设pix₁、pix₂为两个特征点的归一化坐标，则满足

s₂pix₂＝s₁Rpix₁+t

由于测量点摄像机的位置信息可以由街景地图获取，R和t可以根据当前帧车载摄像机到前一帧车载摄像机坐标系间的外参计算得到，s₁、s₂是待求解参数。通过化简得到下式:

其中左侧为零，使用最小二乘法求解s₂，进而得到s₁，获得该像素点三角化后的三维信息，即该点在世界坐标系下的三维坐标。随后，根据车载摄像机拍摄的图像与关键目标对应的真实世界坐标计算正在行驶的车辆相对位姿。利用车辆初始位置和相对位姿变换计算车辆当前时刻的三维位置。利用上述位姿计算得到的三维位置，计算其与先验地图中节点的欧氏距离，选取距离最小的节点作为匹配定位结果节点。

步骤六、如图2所示，横向位置的确定过程采用车道线识别和碰撞检测误差消除的方法。车辆通过车道线位置推算后，通过道路节点匹配和车道碰撞检测误差消除技术对车辆行驶方向的横向误差进行修正。

对道路图像进行前处理，以增强道路的车道线部分。首先，将图像转变为灰度图像，提升运算效率。为减轻道路图像对车道线识别的影响，使用3×3的高斯滤波器进行平滑。对于窗口模板大小为(2k+1)×(2k+1)，各个元素值H_i,j计算公式如下：

利用核大小为1×1的结构化元素进行膨胀和膨胀的形态学处理。使用基于图像梯度的Canny算法进行图像的边缘提取，其中滞后性阈值设置为1和2。使用Sobel算子来计算图像梯度，Sobel算子的孔径大小设置为3。利用非极大值抑制来去除非边缘的噪点并细化边缘，使用可变阈值的方法进行分割并将边缘和背景区分，采用整体图像的灰度值为高阈值，低阈值根据高阈值进行变换。

使用Hough变换检测经过二值化后图像中的车道线，并将角度阈值设置为10度到170度之间，将图像直线映射到参数空间。遍历图像的非零像素点进行参数投票，找出参数空间大于110阈值的参数点。由于车道线会检测出多条直线，采用K-Means算法进行聚类从而获取最终的车道线检测结果，相邻两条直线相对角度小于20度即为相似。

根据车道线检测结果和车辆的边缘信息来推断车辆所属的车道。借助车辆的边缘信息在图像上划定车辆的前进区域和中心线，然后对出现在中心线两侧的车道线进行计数。若存在车道线在车辆的前进区域内部，则推断车辆在车道中心节点，否则推断车辆在车道线节点。除非有特殊情况，在道路上正常行驶的车辆不会行驶到车道线上或者护栏以外，因此借助车道线识别和碰撞检测误差消除方法匹配最优的车道节点。接下来定位给过程采用车道检测误差消除技术结合道路节点数据完成，目的在于将车辆准确匹配到车道上。若在上述车道检测步骤推断出车辆在车道中心节点，则计算当前车辆与先验地图中车辆位置为圆心、固定半径内所有车道中心节点的欧氏距离，选取欧氏距离最小的节点视为当前车辆在该车道的准确位置。若推断出车辆在车道线节点，说明车辆目前正在进行变道操作，则计算当前车辆与先验地图中固定半径内所有的车道线节点的欧氏距离，选取欧氏距离最小的节点视为当前车辆在车道线的准确位置。

对于本领域普通技术人员而言，可以根据上述实施例的各种方法中全部步骤或部分步骤加以改进或变换，凡依本专利所说的变换和修改，皆属于本发明所附权利要求的保护范围内。

Claims (1)

1.一种基于GPS和普通导航地图的车辆高精度定位方法，利用车载摄像机、惯性测量单元、车载GPS模块和车规级工控机进行定位，其特征在于：具体方法包括以下步骤：

A、利用车载GPS模块确定地图的初始定位节点

A1、利用车载GPS模块获取车辆的实时原始数据；所述原始数据包括经纬度、海拔高度和航向角，随后由车规级工控机的计算单元对原始数据进行解码并判断GPS模块状态；在GPS模块正常采集数据后，使用递推平均值数字滤波算法对解码后的原始数据进行滤波处理，去除因解码后的原始数据中噪声引起的误差，使得解码后的原始数据更加平滑；递推平均滤波公式如下：

其中，x表示滤波后的GPS数据，x_i表示第i条解码后的原始数据，N表示序列长度；

A2、根据滤波后的GPS数据进行坐标转换，获取车辆初始位置的三维坐标，并计算与先验地图节点的欧式距离，选取最近的道路节点作为初始的定位节点；在得到滤波后的经纬度、海拔高度和航向角后，获得车辆的初始位置，并计算与先验地图中节点的欧氏距离，选取距离最小的节点作为匹配定位结果节点；初始定位位置和地图节点的欧氏距离计算公式如下：

其中，ρ为欧氏距离，

分别为先验地图中第i个节点P_i的X轴、Y轴、Z轴的坐标值，x₂,y₂,z₂分别为车辆初始定位位置的X轴、Y轴、Z轴的坐标值；

选取车辆GPS初始定位位置与先验地图中欧氏距离最小的节点，将该节点作为初始定位节点P_init，并将该节点的三维坐标作为车辆在世界坐标系下的初始坐标；

B、利用车载摄像机和普通导航地图获取道路数据

对车载摄像机进行标定，并采集车辆周围的街景图像，将采集到的数据存储在车规级工控机的计算单元开辟的vector格式数据库中；所述车载摄像机为环视车载摄像机；

B1、对车载摄像机进行标定，获取车载摄像机的内参矩阵、外参矩阵、畸变系数，并使用针孔相机模型校正摄像机得到的图像；使用内部参数获取车载摄像机从三维空间到二维图像的投影关系，使用外部参数获取车载摄像机坐标与世界坐标系之间相对位置关系；以车辆的车头方向为Y轴正向，横向向右的方向为X轴正向，垂直水平面朝上的方向为Z轴正向，假设车载摄像机与大地的坐标系变换公式为：

其中，α为车载摄像机坐标系绕X轴旋转的角度，β为车载摄像机坐标系绕Y轴旋转的角度，γ为车载摄像机坐标系绕Z轴旋转的角度，x,y,z分别为车载摄像机坐标系下一点p₀的三维坐标，x',y',z'分别为点p₀对应世界坐标系下的点p'₀的三维坐标；

B2、道路数据具体的采集储存过程为：车辆在道路上以固定的车速进行行驶，车载摄像机以固定采样频率采集行驶方向和垂直行驶方向的视觉场景图像，视觉场景图像的分辨率为1280*720；随后，将采集的视觉场景图像进行压缩，并将压缩后的视觉场景图像存储在数据库中；

C、识别和提取道路关键目标

C1、将车载摄像机拍摄的视觉场景图像数据和普通导航地图视觉场景图像输入已经完成训练的轻量化目标检测模型，对关键目标进行识别和提取，所述关键目标包括交通标志、广告牌和文字标牌；所述交通标志包括禁令标志、警告标志、指示标志、指路标志、旅游区标志、道路施工安全标志和辅助标志；

所述轻量化目标检测模型为在YOLO5s网络模型的基础上添加多尺度通道注意力模块和注意力特征融合模块；轻量化目标检测模型的损失函数分为位置损失函数L_CIoU、置信度损失函数L_obj和分类损失函数L_cls，总损失函数为三部分的和；位置损失函数采用CIoU损失函数，置信度损失函数和分类损失函数均采用二分类的交叉熵损失函数；

C2、轻量化目标检测模型分别对车载摄像机采集图像和普通导航街景图像中的关键目标进行检测，通过检测结果的标签文件获取目标区域像素坐标值，并为所述关键目标设置单独的索引号以便后期的查询；关键目标的图像像素坐标和索引号组成目标关键信息；根据检测的目标类别，将交通标志类型的目标关键信息、所属视觉场景图像和文字标牌的目标关键信息分别保存在数据库中；

D、关联关键目标数据

D1、提取普通导航地图中固定半径范围内的所有目标关键信息，并将车载摄像机拍摄的视觉场景图像中提取的关键目标依次与其进行视觉特征提取和匹配；

根据GPS模块确定的初始定位位置的结果，获取普通导航地图中以R_s为半径的所有关键目标信息，将其设为待匹配的关键目标；将车载摄像机拍摄的视觉场景图像通过轻量化目标检测模型提取的关键目标依次与普通导航街景地图提取的关键目标进行视觉特征提取和匹配，具体方法如下：

通过Hesse矩阵的判别式来计算特征值，使用高斯差分算法进行计算；在特征提取后采用特征向量之间的相似性来进行特征匹配，使用欧式距离作为判断特征向量相似性的指标，表示如下：

其中，d_ii为特征点u_i和v_i的欧氏距离，

为特征点u_i和v_i的特征向量；

D2、利用RANSAC算法对匹配的点对进行筛选，筛选后的特征点数目大于阈值则判定待匹配的关键目标匹配成功，记录该关键目标的信息；具体方法如下：

利用RANSAC算法进行筛选，找出匹配的内点和偏离模型正常范围的外点；随机从特征点对匹配集中选取4对匹配点计算相对变换的矩阵来判断初始匹配集中哪些特征点对匹配符合条件，迭代n次后选取内点最多的结果作为最后的匹配结果；利用RANSAC算法剔除错误的特征点对，记录最终剩余特征点对的个数N_r；设置匹配成功的阈值为N_i；如果关键目标中正确匹配的特征点对N_r大于阈值N_i则判定该关键目标匹配成功，否则匹配失败继续匹配新的关键目标；

D3、基于车载摄像机拍摄的视觉场景图像采集的文字特征进行扫描识别，将结果与固定半径范围内的普通导航地图中文字识别结果进行匹配，获取关联的数据；

匹配成功后分别获取在普通导航街景地图中关键目标拍摄点的三维坐标和该目标所属的场景图像，以及在车载摄像机中关键目标所属的视觉场景图像；将含有广告牌、文字标牌的关键目标进行文字识别，并与普通导航地图所提供的信息进行文字扫描匹配；根据扫描匹配的结果，记录车载摄像机和普通导航地图的目标内部文字一致的广告牌、文字标牌的关键目标的索引，从而获取关联数据；

E、计算车辆位姿

通过普通导航地图中关键目标像素位置和拍摄点的三维位置信息进行坐标转换，并结合基于文字扫描匹配结果获取关键目标对应的真实世界坐标；

基于导航地图提供关键目标位置信息，利用匹配成功的关联数据对当前车辆的位姿进行计算；

通过普通导航地图街景图像目标像素位置和拍摄点的三维世界坐标进行转换，获取关键目标对应的真实世界坐标；设pix₁、pix₂分别为两个特征点的归一化坐标，则满足下式：

s₂pix₂＝s₁Rpix₁+t

由于测量点车载摄像机的位置信息由街景地图获取，R和t为根据当前帧车载摄像机到前一帧车载摄像机坐标系间的外参计算得到，s₁、s₂是待求解参数；通过化简得到下式：

其中左侧为零，使用最小二乘法求解s₂，进而得到s₁，获得两个特征点对应像素坐标三角化后的三维信息；

根据车载图像与关键目标对应的真实世界坐标和在图像中的像素坐标计算当前状态的车辆位姿，具体方法如下：

根据车载摄像机拍摄的图像与关键目标对应的真实世界坐标计算正在行驶的车辆相对位姿；利用车辆初始位置和相对位姿变换计算车辆当前时刻的三维位置；利用上述位姿计算得到的三维位置，计算其与先验地图中节点的欧氏距离，选取距离最小的节点作为匹配定位结果节点；

F、基于车道线检测消除横向位置误差

F1、对视觉场景图像进行增强处理，使用Sobel算子计算图像梯度，利用非极大值抑制来去除非边缘的噪点并细化边缘，使用可变阈值的方法进行分割并将边缘和背景区分，具体方法如下：

对道路图像进行前处理，以增强道路的车道线部分；首先，将图像转变为灰度图像，使用高斯滤波器进行平滑；然后，使用基于图像梯度的Canny算法进行图像的边缘提取；使用Sobel算子计算图像梯度，使用可变阈值的方法进行分割并将边缘和背景区分，采用整体图像的灰度值为高阈值，低阈值根据高阈值进行变换；

F2、使用Hough变换检测图像中的车道线，对检测出的多条车道线基于K-means聚类算法进行聚类，获取最终的车道线检测结果，具体方法如下：

使用Hough变换检测经过二值化后图像中的车道线，将图像直线映射到参数空间，遍历图像的非零像素点进行参数投票，找出参数空间大于阈值的参数点；由于车道线会检测出多条直线，采用K-Means算法进行聚类从而获取最终的车道线检测结果；

F3、根据车道线检测结果和车辆的边缘信息来推断车辆所属的车道，匹配出最优的道路节点，提高车辆的定位精度；

借助车辆的边缘信息在图像上划定车辆的前进区域和中心线，然后对出现在中心线两侧的车道线进行计数；若存在车道线在车辆的前进区域内部，则推断车辆在车道中心节点，否则推断车辆在车道线节点；

借助车道线识别的结果，将车辆所在车道上固定半径范围的地图节点视为待匹配的节点，然后逐一计算当前车辆三维坐标和所有待匹配节点三维坐标的欧氏距离；选取欧式距离最小的地图节点为匹配的最优道路节点，并将该节点的三维坐标位置视为车辆优化后的三维坐标。

Images