CN110411462B - 一种gnss/惯导/车道线约束/里程计多源融合方法 - Google Patents

Abstract

本发明公开了一种GNSS/惯性/车道线约束/里程计多源融合方法，首先利用视觉传感器，采用基于视觉+众包模式离线生成车道线地图数据库。在载体进行实时解算导航定位时，以差分GNSS/INS紧组合为基础，GNSS遮挡严重情况时，通过载体上安装的视觉传感器探测载体和车道线的相对关系，在车道线地图的辅助下，动态加入车道线约束观测方程辅助定位，有效抑制侧向和高程两个方向上的位置发散，并通过轮式里程计提供车辆前进方向的速度观测值，以及载体上方和道路前进方向的可观测卫星，显著改善载体前向误差。本发明可以在城市典型的动态复杂环境下，有效的控制三个方向上的位置误差漂移，是实现城市复杂环境下厘米级定位的有效方案。

Description

一种GNSS/惯导/车道线约束/里程计多源融合方法

技术领域

本发明涉及组合导航领域，尤其涉及一种GNSS/惯导/车道线约束/里程计多源融合方法。

背景技术

随着我国北斗卫星导航***区域服务***的建成运行，美国GPS、俄罗斯GLONASS的现代化，以及欧盟Galileo、日本QZSS、印度IRNSS***的建设，GNSS(GNSS,GlobalNavigation Satellite System)正朝多频多***的方向蓬勃发展。然后，由于GNSS存在遮挡、干扰和欺骗三大脆弱性问题，无法在复杂环境中提供可靠、连续的定位服务。因此，集成多种不同导航***与传感器、融合一切可用信号源，即插即用的方式的全源定位与导航被提出，旨在融合所有有效信息，通过先进的自适应导航算法，实现任何平台、任何环境下的高可用、高可靠的无缝导航，从而摆脱对GNSS的过度依赖。

高精度的定位定姿技术在测绘移动测量领域和大众位置服务领域存在着巨大的需求。传统的RTK/INS后处理组合导航模式可基本满足移动测量的需求，但近年来，以自动驾驶为代表的智能产业的兴起对定位导航提出了更高的要求。目前Google、Waymo、Tesla、百度、京东等科技公司已基本具备开阔环境或限定场景下的无人驾驶能力。然而，城市环境复杂多变，难以预测，在不确定性大量以及知识不完备的情况下，自动驾驶面临着巨大风险。其中，城市环境下的精准定位有事自动驾驶能否实现大范围落地应用的关键。目前，自动驾驶定位技术均采用多传感器协同工作来完成高精度定位，包括全球卫星导航***(GNSS)接收机、惯性传感器、激光雷达、视觉传感器、里程计等。然而，GNSS信号容易受到城市楼群、高架隧道的遮挡失去定位能力，即使能够锁定卫星信号，也往往表现出低信噪比、多周跳、多粗差、模糊度固定错误的问题，导致数据处理难度极大；传统的惯导/里程计航位推算技术无法保持长时间高精度的自主定位能力；此外，激光雷达/视觉传感器属于匹配定位技术，受到道路车辆、行人动态物体的干扰，同时，低光照、弱纹理、运动模糊等环境因素影响严重，如何解决这些难点问题，实现多源异质数据融合的高精度定位，是城市环境下自动驾驶亟待解决的瓶颈问题之一。

针对上述问题，本发明给出了一种GNSS/惯导/车道线约束/里程计多源融合的方法，以差分GNSS/INS紧组合为基础，在车道线地图辅助下，提供车道线侧向、高程约束观测方程，通过轮式里程计和载体上方及道路前进方向的可观测卫星提供载体前向约束，从而在三个方向上抑制惯导误差发散，提供可靠、连续的高精度位姿。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种GNSS/惯导/车道线约束/里程计多源融合方法。

本发明解决其技术问题所采用的技术方案是：

本发明提供一种GNSS/惯导/车道线约束/里程计多源融合方法，该方法中，通过视觉传感器采集图像，离线生成车道线地图数据库；在载体进行实时解算导航定位时，以差分GNSS/INS紧组合为基础，GNSS遮挡时，通过载体上安装的视觉传感器探测载体和车道线的相对关系，在车道线地图的辅助下，动态加入车道线约束观测方程辅助侧向、高程定位；并利用载体上方和道路前进方向的可观测卫星形成差分观测值参与紧组合，约束载体前向误差发散。

进一步地，本发明的该方法具体包括以下步骤：

步骤1、对实时采集的影像预处理得到车道线影像的俯视影像，对影像进行颜色空间转换分离出车道线，并基于滑动窗口的灰度直方图统计确定车道线的坐标，最后利用基于匀速假设的卡尔曼滤波对车道线进行跟踪，得到连续平滑的相机中心距离左右影像的距离；

步骤2、利用八叉树数据结构快速搜索到惯导预报位置的最小体素，利用欧氏几何距离法找到最邻近车道线节点，并对节点进行航向/拓扑一致性检验，检验通过后根据曲率返回最佳匹配的P_L1、P_L2、P_R1、P_R2；

步骤3、根据惯导预报位置，经过车高和与相机的外参改正后，位于车道面上的假设，构建车道线高程约束观测方程；

步骤4、将P_L1、P_L2、P_R1、P_R2四个点抬高至相机中心处，根据相机中心预报值对车道线的距离应等于观测距离的假设构建相机中心与车道线的侧向约束观测方程，并利用链式法则，将其转换至惯导中心；

步骤5、根据载体在直线运动时打滑较少且转弯时速度较慢的假设，构建里程计前向约束的观测方程；

步骤6、根据车道线高程、侧向约束的方程以及里程计的前向约束观测方程，实现多源融合观测更新。

进一步地，本发明的步骤1中进行车道线跟踪的方法具体为：

步骤1.1、预处理：通过Matlab标定工具箱对单目相机进行单目标定，获得相机内参数，利用相机内参数对单目相机采集的车道线图像进行畸变校正；根据单目相机安装的位置和视野范围，划定感兴趣区域，排除其他无关区域干扰，并利用单应性变换，将车道线恢复到俯视状态下；

步骤1.2、车道线提取：输入车道线正射影像，通过颜色和边缘信息的阈值分割，将车道线正射影像二值化；对图像底部划分像素窗口，并对窗口灰度值进行从左到右的直方图统计，采用高斯核平滑得到灰度均值曲线图，提取波峰位置作为车道线位置；

步骤1.3、车道线跟踪：采用卡尔曼滤波对车道线进行跟踪，使用速度模型建立前后历元的关系，状态方程为：

其中，q_x和q_v分别是位置x和速度v的过程噪声，k为时刻；

观测方程为：

y_k+1＝x_k+1+ε

其中，y_k+1为当前提取到的车道线位置；ε表示提取车道线位置的观测噪声；

滤波后，得到每个窗口i的位置

再将连续窗口的位置进行二次曲线拟合，从而得到全时段连续平滑的车道线提取，实现对车道线虚线、斑马线的平滑过度；成功提取车道线后，将获得的底部左右车道线在相机坐标系下的x坐标，即相机和左右车道线的距离。

进一步地，本发明的步骤2中八叉树辅助的车道线快速搜索的具体方法为：

步骤2.1、最邻近车道线节点搜索：根据惯导预报位置，从车道线的空间八叉树存储结构寻找预报点对应的子节点体素；找到体素后，遍历体素中的所有车道线坐标点，根据欧式几何距离法找到距离最邻近的点；

步骤2.2、一致性判断：根据惯导预报的位置点的姿态中的航向信息得到载体的前进方向，通过前进方向和搜索到的车道线节点的航向进行一致性判断，若两者航向之差大于阈值，则表明搜索到的车道线节点有误；

根据历史时刻成功搜索到的车道线节点和当前搜索到的车道线节点的拓扑关系检验，判断历史节点和当前节点是否具备邻接拓扑关系，若不具备，则表明搜索到的车道线节点有误；

反之，表明搜索到的车道线节点正确。

进一步地，本发明的步骤3中构建的车道线高程约束观测方程为：

其中，

为预报距离，d_I为常值，H_lane为高程约束的系数矩阵，δr^e、δv^e、φ、a^b、ε^b分别表示位置、速度、姿态误差以及加速度计零偏、陀螺零偏；高程约束的系数矩阵表示为：

其中，(A,B,C)为平面系数。

进一步地，本发明的步骤4中构建相机中心与车道线的侧向约束观测方程为：

其中，f_L、f_R表示观测到的离左右车道线的距离，

表示预报位置距离搜索到的左右车道线的距离，

表示观测值对位置、姿态的偏导数。

进一步地，本发明的步骤5中构建的里程计前向约束的观测方程为：

其中，δv^b表示里程计观测值和惯导预报的b系下的速度的差值、

表示惯导预报的ECEF系相对于b系的旋转矩阵、

表示惯导预报的ECEF系下的速度、l^b表示里程计与惯导的杆臂、

表示地球自转角速度。

进一步地，本发明的步骤6中融合GNSS/车道线约束/里程计融合后，得到的观测方程为：

其中，v_P和v_L表示载波和相位观测值，

表示站间星间双差观测值的方向余弦向量，

表示天线与惯导在ECEF系下的杆臂，δN为双差模糊度状态参数，ε_P、ε_L、ε_f、ε_d、

分表表示载波、相位、车道线侧向观测值、车道线高程观测值以及里程计观测值的观测噪声。

进一步地，本发明的步骤6中进行多源融合的方法具体为：

动态地选择是否加入车道线、里程计约束，在GNSS满足观测条件时，仅用GNSS/INS进行定位，在GNSS受到遮挡或中断时，加入里程计约束，并根据车道线探测情况决定是否加入车道线约束。

本发明产生的有益效果是：本发明的GNSS/惯导/车道线约束/里程计多源融合方法，1)充分利用了各个传感器信息定位互补特性，从而实现三个方向上的定位约束，极大抑制了惯导在各个方向的误差发散。2)不同传感器，不同数据之间的约束互不影响，且可相互约束检校，有双重约束保障，定位可靠性高。3)根据典型的城市环境的不同特性，可动态选择是否加入该种信息的观测约束，定位手段灵活。4)里程计和视觉传感器成本低，且目前大多车辆载体已本身具备，故该融合定位方法应用便捷，且基本无需额外增加成本。5)通过三个方向的约束，可使得该定位方法允许的GNSS中断时间延展至五分钟左右，在这五分钟的中断时间里，定位精度仍可维持在分米至厘米级别。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1为本发明实例的GNSS/惯导/车道线约束/里程计多源融合算法总流程图；

图2为本发明实例的GNSS/惯导紧组合结构图；

图3为本发明实例的车道线提取流程图；

图4为本发明实例的最邻近车道线搜索流程图；

图5为本发明实例的车道线约束示意图；

图6为本发明实例的多源融合定位流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明的整体滤波框架为GNSS/INS紧组合扩展卡尔曼滤波，紧组合结构如图2所示，导航坐标系选为ECEF系，相应的SINS机械编排也在ECEF系下进行。紧组合中，GNSS和SINS的原始观测值共同输入到一个Kalman滤波器中，联合估计导航参数(位置、速度和姿态)、SINS***误差以及GNSS相关参数(模糊度)，并且采用闭环修正技术，对SINS***误差进行反馈校正。GNSS/SINS紧组合状态模型和观测模型，分别如下：

δz＝HδX+η (2)

式(1)中，δX_SINS＝(δr^e δv^e φ a^b ε^b)^T，

分别为SINS和GNSS的状态参数，由于采用了双差的定位模式，GNSS接收机钟差已被消去，只留下了站间差分模糊度。F为状态微分方程系数矩阵，w为过程噪声；式(2)中，δz为观测值残差，H为设计矩阵，η为观测噪声。

按照上述紧组合数学模型，采用Kalman滤波进行估计，其一般解算步骤包括一步状态预测和观测更新两部分，具体如下：

一步状态预测：

观测更新：

式(3)中，X_k,k-1为预报状态，Φ_k,k-1为状态转移矩阵，Q_k-1为过程噪声，Δt_k为两个历元间的时间间隔，P_k,k-1为预报方差。式(4)中，K_k为增益系数，R_k为观测噪声，X_k和P_k为滤波状态及其方差。

以上紧组合数学模型和Kalman滤波一般解算步骤是本发明算法的基础，下面将结合图1所示的技术路线，对本发明中各模块的关键技术及其实施方法展开详细叙述。

一、基于卡尔曼滤波的车道线提取

车道线提取方法可分基于道路特征和道路模型的车道线提取方法和基于深度学习的车道线提取方法两大类。深度学习具有较好的检测效果，但依赖大量的样本数据进行模型训练。当前，我们仍以传统的方法为主，利用车道线特征、颜色、结构等区别于道路背景的信息，来分离得到车道线，并采用滑动窗口的卡尔曼滤波方法进行改进，流程如图3所示，主要包括预处理、车道线提取和车道线跟踪三大模块。

1)预处理

在采集数据前，通过Matlab标定工具箱对单目相机进行单目标定，获得相机内参数，包括焦距f_x、f_y、像主点位移c_x、c_y以及相机的径向畸变系数和切向畸变系数。预处理时，利用上述参数对单目相机采集的车道线图像进行畸变校正。根据单目相机安装的位置和视野范围，划定感兴趣区域，排除其他无关区域干扰，并利用单应性变换公式(5)，将车道线恢复到俯视状态下，H为对应的单应性矩阵，u,v为像素坐标系下的坐标，[X,Y,Z]为俯视状态下的对应点坐标，这样可便于后续车道线提取处理。

2)车道线提取

输入车道线正射影像，通过颜色和边缘信息的阈值分割，将车道线正射影像二值化。由于车道线通常为白色和黄色，且车道线为引导驾驶员开车，通常对光反射强度较高，颜色较亮，故综合使用LAB颜色空间的B通道、HSL和HSV颜色空间的亮度通道检测车道线，以将车道线和背景进行分离。二值化后，对图像底部划分像素窗口，并对窗口灰度值进行从左到右的直方图统计，然后采用高斯核平滑得到灰度均值曲线图，提取波峰位置作为车道线位置。

3)车道线跟踪

由于车道线在车辆行驶过程中是连续缓慢变化的，根据这一性质，可采用卡尔曼滤波对车道线进行动态、连续、平滑跟踪。

使用速度模型建立前后历元的关系，状态方程如下，式中，q_x和q_v分别是位置x和速度v的过程噪声：

观测方程如下，y_k+1为当前提取到的车道线位置：

y_k+1＝x_k+1+ε (7)

滤波后，得到每个窗口i的位置

最后再将连续窗口的位置进行二次曲线拟合，从而得到全时段连续平滑的车道线提取，实现对车道线虚线、斑马线的平滑过度。

成功提取车道线后，将获得的底部左右车道线在相机坐标系下的x坐标，即相机和左右车道线的距离l_L和l_R，反馈给GNSS/INS滤波器。

二、八叉树辅助的车道线快速搜索

车道线地图包含车道的全部信息，包括坐标、航向、曲率、颜色、拓扑关系等等。为提高车道线搜索效率，在对车道线地图数据存储时，采用八叉树结构，在坐标域进行组织管理，对点云坐标建立空间索引，实现车道线坐标的空间聚类、划分。八叉树的每个结点表示一个正方体的体积元素，父结点可分为八个子节点，如此分割下去，直到不可分割。存储车道线地图时，将空间八叉树最小体素的大小设置为10m，那么对于40km长的车道来说，最多也只需12次搜索就可知道车道线坐标位于哪个体素。

1)最邻近车道线节点搜索

根据惯导预报位置，如图5所示的点

从车道线的空间八叉树存储结构寻找预报点对应的子节点体素。找到体素后，遍历体素中的所有车道线坐标点，根据欧式几何距离法找到距离最邻近的点。

2)一致性判断

根据惯导预报的位置点的姿态中的航向信息得到载体的前进方向，通过前进方向和搜索到的车道线节点的航向进行一致性判断，若两者航向之差大于阈值，则表明搜索到的车道线节点有误。

根据历史时刻成功搜索到的车道线节点和当前搜索到的车道线节点的拓扑关系检验，判断历史节点和当前节点是否具备邻接拓扑关系，若不具备，则表明搜索到的车道线节点有误。

若通过上述一致性判断，则表明搜索到的车道线节点正确。由于载体实际上所处的车道线是左右两段连续的曲线，而目前搜索到的仅为最邻近的左右两个车道线节点，故首先需根据节点的曲率信息，将节点向前向后外推n个节点，n取决于节点曲率的大小，曲率大则n也可适当放大。外推后可得到左车道线的2个节点P_L1、P_L2以及右车道线的2个节点P_R1、P_R2，如图5所示。

三、车道线辅助GNSS/INS高程定位约束

已经得到了最邻近的左车道线节点P_L1、P_L2和右车道线的2个节点P_R1、P_R2，理论上，如果惯导预报位置没有误差，则其中心经过车高和与相机的外参改正后应位于车道面上，由此可建立高程约束。

假设惯导中心到地面的距离事先已知，设为d_I，左右车道线的四点坐标也已知，为(P_L1,P_L2,P_R1,P_R2)，地平面可表示为A·X+B·Y+C·Z+1＝0，将车道线的四点坐标代入，求得平面系数(A,B,C)，根据点到面的距离公式，可得惯导中心到地平面的距离为：

因为车辆一定在路面上方，所以式中的绝对值符号可以去掉，最终表示为惯导位置误差的观测方程：

式中，

为预报距离，将惯导位置的预报值代入(8)式得到，H_lane为高程约束的系数矩阵，表示如下：

最后形成的观测方程如下：

d_I为常值，如果事先没有测量出来，也可以实时计算。在GNSS信号良好的情况下，GNSS/SINS组合可以得到厘米级的定位结果，此时直接计算惯导中心到地面的垂直距离，保存以后为后续高程约束所用。

四、车道线辅助GNSS/INS侧向定位约束

侧向约束方程需要通过相机中心做为媒介，通过车道线提取、单应性变换，我们得到了相机中心到车道线的水平距离，左右车道线的距离分别记为d_L和d_R。假设相机中心的位置为P_c，它到路面存在一个高度值，因此，要计算相机到车道线的水平距离，相机中心必须和路面在同一个平面上。

为统一计算，需再构建观测方程时，将路面抬高到相机中心处，具体做法如下：1)获得相机中心到路面的距离d_c。步骤4中，我们已经得到了惯导中心到路面的距离，而惯导与相机的空间又是在采集数据之前提前标定好的，属于已知。因此，相机中心到路面的距离

为两者的垂向距离；2)将路面坐标(P_L1,P_L2,P_R1,P_R2)抬高，由于地平面系数

即为该平面的单位法向量，因此，路面坐标加上同一个向量增量

得到新的路面坐标(P′_L1,P′_L2,P′_R1,P′_R2)即为抬升至相机中心的车道线。

此时，通过构造两个向量，叉乘得到两个向量围成的四平行面积，然后除以底边，计算得到高，即为相机中心到车道线的水平距离：

(P_c-P′_L1)×m_L向量叉乘展开表示为：

对(12)式两边距离平方后做为新观测量，使用MATLAB代数工具箱，得到平方距离f_L＝(d_L·||m_L||)²对P_c的偏导数：

式中，M₁、M₂和M₃表示如下：

将相机中心坐标P_c和惯导中心坐标P_SINS关系为：

式中，

l_b是相机中心在惯导坐标系下的杆臂分量。由此得到P_c对惯导位姿误差的偏导数为：

式中，r^e＝P_SINS，为了与状态方程一致。根据链式法则，由(14)式和(17)式，得到平方距离f对惯导位姿误差的偏导数：

最后，得到观测方程为：

五、里程计辅助GNSS/INS前向定位约束

采用轮式里程计进行前向约束有两个假设：1)运动载体没有侧滑，沿载体坐标系前进方向运动，即侧向速度为零；2)载体贴着地表运动，没有上下波动，即垂向速度为零。

由里程计得到的前向速度为：

式中，

是里程计速度转到惯导本体系下的速度，

是里程计速度，只在前向有速度，其余两轴为零，

l^b是里程计中心在惯导本体系下的位置矢量，即里程计杆臂改正。

从该公式出发对里程计约束的观测模型进行推导，位置和速度状态均以SINS中心为基准。采用扰动法，可得：

此时，推导所得的观测方程如下：

以上为严格推导的观测模型，但可根据实际情况用以下模型进行简化。

当车辆直线行驶时，惯导系没有相对于地面转动，

接近于零，此时，里程计速度等于惯导系下的前进速度。当车辆转弯时，

计算得到的

会在测向(X轴)出现速度分量，与非完整性约束假设不一致，这是因为车辆转弯时以后轮为定轴转动，不在后轮轴面上的点都会出现转动速度，但实际中地面车辆转弯比较缓慢，该速度分量很小，稍微放大点观测噪声即可，可直接认为v_SINS＝v_DMI。此时观测方程可简化成：

式中，

和

为当前时刻机械编排得到的值，下标2表示取第二行为：

六、多源融合观测更新

上述步骤中，我们已经得到了车道线高程、侧向约束的方程以及里程计的前向约束观测方程。整个融合定位***以差分GNSS/SINS紧组合为基础，动态的加入车道线约束观测方程和里程计观测方程，如图6所示。

当GNSS卫星数较多以及PDOP值较小时，说明GNSS定位几何条件满足要求，此时，可以删去车道线和里程计的观测，状态方程仍然不变。

当GNSS卫星数较少时并且车道线检测成功时，向观测方程中加入车道线的侧向和高程约束方程。

在道路两侧高楼林立的城市场景下，GNSS信号遮挡严重，但道路前后向遮挡较少，GNSS卫星仍可见，此时，选择载体上方，即高度角最高的卫星作为基准星，将基准星和道路前后向的可见卫星组成双差观测值参与定位解算，并同时加入里程计的前向约束。当车辆处于高架桥下，隧道等场景中，GNSS卫星将全部不可见，此时前向只采用里程计误差约束。

完整建立GNSS/车道线约束/里程计融合的观测方程如下：

v_P和v_L表示载波和相位观测值，ε为各观测值噪声。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (8)

1.一种GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，该方法中，通过视觉传感器采集图像，离线生成车道线地图数据库；在载体进行实时解算导航定位时，以差分GNSS/INS紧组合为基础，GNSS遮挡时，通过载体上安装的视觉传感器探测载体和车道线的相对关系，在车道线地图的辅助下，动态加入车道线约束观测方程辅助侧向、高程定位；并利用载体上方和道路前进方向的可观测卫星形成差分观测值参与紧组合，约束载体前向误差发散；该方法具体包括以下步骤：

步骤1、对实时采集的影像预处理得到车道线影像的俯视影像，对影像进行颜色空间转换分离出车道线，并基于滑动窗口的灰度直方图统计确定车道线的坐标，最后利用基于匀速假设的卡尔曼滤波对车道线进行跟踪，得到连续平滑的相机中心距离左右影像的距离；

步骤2、利用八叉树数据结构快速搜索到惯导预报位置的最小体素，利用欧氏几何距离法找到最邻近车道线节点，并对节点进行航向/拓扑一致性检验，检验通过后根据曲率返回最佳匹配的P_L1、P_L2、P_R1、P_R2；

步骤3、根据惯导预报位置，经过车高和与相机的外参改正后，位于车道面上的假设，构建车道线高程约束观测方程；

步骤4、将P_L1、P_L2、P_R1、P_R2四个点抬高至相机中心处，根据相机中心预报值对车道线的距离应等于观测距离的假设构建相机中心与车道线的侧向约束观测方程，并利用链式法则，将其转换至惯导中心；

步骤5、根据载体在直线运动时打滑较少且转弯时速度较慢的假设，构建里程计前向约束的观测方程；

步骤6、根据车道线高程、侧向约束的方程以及里程计的前向约束观测方程，实现多源融合观测更新。

2.根据权利要求1所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤1中进行车道线跟踪的方法具体为：

步骤1.1、预处理：通过Matlab标定工具箱对单目相机进行单目标定，获得相机内参数，利用相机内参数对单目相机采集的车道线图像进行畸变校正；根据单目相机安装的位置和视野范围，划定感兴趣区域，排除其他无关区域干扰，并利用单应性变换，将车道线恢复到俯视状态下；

步骤1.2、车道线提取：输入车道线正射影像，通过颜色和边缘信息的阈值分割，将车道线正射影像二值化；对图像底部划分像素窗口，并对窗口灰度值进行从左到右的直方图统计，采用高斯核平滑得到灰度均值曲线图，提取波峰位置作为车道线位置；

步骤1.3、车道线跟踪：采用卡尔曼滤波对车道线进行跟踪，使用速度模型建立前后历元的关系，状态方程为：

其中，q_x和q_v分别是位置x和速度v的过程噪声，k为时刻；

观测方程为：

y_k+1＝x_k+1+ε

其中，y_k+1为当前提取到的车道线位置；ε表示提取车道线位置的观测噪声；

滤波后，得到每个窗口i的位置

再将连续窗口的位置进行二次曲线拟合，从而得到全时段连续平滑的车道线提取，实现对车道线虚线、斑马线的平滑过度；成功提取车道线后，将获得底部左右车道线在相机坐标系下的x坐标，即相机和左右车道线的距离。

3.根据权利要求2所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤2中八叉树辅助的车道线快速搜索的具体方法为：

步骤2.1、最邻近车道线节点搜索：根据惯导预报位置，从车道线的空间八叉树存储结构寻找预报点对应的子节点体素；找到体素后，遍历体素中的所有车道线坐标点，根据欧式几何距离法找到距离最邻近的点；

步骤2.2、一致性判断：根据惯导预报的位置点的姿态中的航向信息得到载体的前进方向，通过前进方向和搜索到的车道线节点的航向进行一致性判断，若两者航向之差大于阈值，则表明搜索到的车道线节点有误；

根据历史时刻成功搜索到的车道线节点和当前搜索到的车道线节点的拓扑关系检验，判断历史节点和当前节点是否具备邻接拓扑关系，若不具备，则表明搜索到的车道线节点有误；

反之，表明搜索到的车道线节点正确。

4.根据权利要求1所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤3中构建的车道线高程约束观测方程为：

其中，

为预报距离，d_I为常值，H_lane为高程约束的系数矩阵，δr^e、δv^e、φ、a^b、ε^b分别表示位置误差、速度误差、姿态误差以及加速度计零偏、陀螺零偏；高程约束的系数矩阵表示为：

其中，A,B,C为平面系数。

5.根据权利要求2所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤4中构建相机中心与车道线的侧向约束观测方程为：

其中，f_L、f_R表示观测到的离左右车道线的距离，

表示预报位置距离搜索到的左右车道线的距离，

表示观测值对位置、姿态的偏导数。

6.根据权利要求5所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤5中构建的里程计前向约束的观测方程为：

其中，δv^b表示里程计观测值和惯导预报的b系下的速度的差值、

表示惯导预报的ECEF系相对于b系的旋转矩阵、

表示惯导预报的ECEF系下的速度、l^b表示里程计与惯导的杆臂、

表示地球自转角速度。

7.根据权利要求6所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤6中GNSS/车道线约束/里程计融合后，得到的观测方程为：

其中，v_P和v_L表示载波和相位观测值，

表示站间星间双差观测值的方向余弦向量，

表示天线与惯导在ECEF系下的杆臂，δN为双差模糊度状态参数，ε_P、ε_L、ε_f、ε_d、

分别表示载波的观测噪声、相位的观测噪声、车道线侧向观测值的观测噪声、车道线高程观测值的观测噪声以及里程计观测值的观测噪声。

8.根据权利要求1所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤6中进行多源融合的方法具体为：

动态地选择是否加入车道线、里程计约束，在GNSS满足观测条件时，仅用GNSS/INS进行定位，在GNSS受到遮挡或中断时，加入里程计约束，并根据车道线探测情况决定是否加入车道线约束。

Images