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

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

Info

Publication number
CN110411462B
CN110411462B CN201910659041.4A CN201910659041A CN110411462B CN 110411462 B CN110411462 B CN 110411462B CN 201910659041 A CN201910659041 A CN 201910659041A CN 110411462 B CN110411462 B CN 110411462B
Authority
CN
China
Prior art keywords
lane line
constraint
gnss
inertial navigation
observation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910659041.4A
Other languages
English (en)
Other versions
CN110411462A (zh
Inventor
张小红
沈燕芬
朱锋
贾俊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN201910659041.4A priority Critical patent/CN110411462B/zh
Publication of CN110411462A publication Critical patent/CN110411462A/zh
Application granted granted Critical
Publication of CN110411462B publication Critical patent/CN110411462B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

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、利用八叉树数据结构快速搜索到惯导预报位置的最小体素，利用欧氏几何距离法找到最邻近车道线节点，并对节点进行航向/拓扑一致性检验，检验通过后根据曲率返回最佳匹配的PL1、PL2、PR1、PR2；
步骤3、根据惯导预报位置，经过车高和与相机的外参改正后，位于车道面上的假设，构建车道线高程约束观测方程；
步骤4、将PL1、PL2、PR1、PR2四个点抬高至相机中心处，根据相机中心预报值对车道线的距离应等于观测距离的假设构建相机中心与车道线的侧向约束观测方程，并利用链式法则，将其转换至惯导中心；
步骤5、根据载体在直线运动时打滑较少且转弯时速度较慢的假设，构建里程计前向约束的观测方程；
步骤6、根据车道线高程、侧向约束的方程以及里程计的前向约束观测方程，实现多源融合观测更新。
进一步地，本发明的步骤1中进行车道线跟踪的方法具体为：
步骤1.1、预处理：通过Matlab标定工具箱对单目相机进行单目标定，获得相机内参数，利用相机内参数对单目相机采集的车道线图像进行畸变校正；根据单目相机安装的位置和视野范围，划定感兴趣区域，排除其他无关区域干扰，并利用单应性变换，将车道线恢复到俯视状态下；
步骤1.2、车道线提取：输入车道线正射影像，通过颜色和边缘信息的阈值分割，将车道线正射影像二值化；对图像底部划分像素窗口，并对窗口灰度值进行从左到右的直方图统计，采用高斯核平滑得到灰度均值曲线图，提取波峰位置作为车道线位置；
步骤1.3、车道线跟踪：采用卡尔曼滤波对车道线进行跟踪，使用速度模型建立前后历元的关系，状态方程为：
Figure GDA0002930001960000031
其中，qx和qv分别是位置x和速度v的过程噪声，k为时刻；
观测方程为：
yk+1＝xk+1+ε
其中，yk+1为当前提取到的车道线位置；ε表示提取车道线位置的观测噪声；
滤波后，得到每个窗口i的位置
Figure GDA0002930001960000041
再将连续窗口的位置进行二次曲线拟合，从而得到全时段连续平滑的车道线提取，实现对车道线虚线、斑马线的平滑过度；成功提取车道线后，将获得的底部左右车道线在相机坐标系下的x坐标，即相机和左右车道线的距离。
进一步地，本发明的步骤2中八叉树辅助的车道线快速搜索的具体方法为：
步骤2.1、最邻近车道线节点搜索：根据惯导预报位置，从车道线的空间八叉树存储结构寻找预报点对应的子节点体素；找到体素后，遍历体素中的所有车道线坐标点，根据欧式几何距离法找到距离最邻近的点；
步骤2.2、一致性判断：根据惯导预报的位置点的姿态中的航向信息得到载体的前进方向，通过前进方向和搜索到的车道线节点的航向进行一致性判断，若两者航向之差大于阈值，则表明搜索到的车道线节点有误；
根据历史时刻成功搜索到的车道线节点和当前搜索到的车道线节点的拓扑关系检验，判断历史节点和当前节点是否具备邻接拓扑关系，若不具备，则表明搜索到的车道线节点有误；
反之，表明搜索到的车道线节点正确。
进一步地，本发明的步骤3中构建的车道线高程约束观测方程为：
Figure GDA0002930001960000042
其中，
Figure GDA0002930001960000043
为预报距离，dI为常值，Hlane为高程约束的系数矩阵，δre、δve、φ、ab、εb分别表示位置、速度、姿态误差以及加速度计零偏、陀螺零偏；高程约束的系数矩阵表示为：
Figure GDA0002930001960000044
其中，(A,B,C)为平面系数。
进一步地，本发明的步骤4中构建相机中心与车道线的侧向约束观测方程为：
Figure GDA0002930001960000051
其中，fL、fR表示观测到的离左右车道线的距离，
Figure GDA0002930001960000052
表示预报位置距离搜索到的左右车道线的距离，
Figure GDA0002930001960000053
表示观测值对位置、姿态的偏导数。
进一步地，本发明的步骤5中构建的里程计前向约束的观测方程为：
Figure GDA0002930001960000054
其中，δvb表示里程计观测值和惯导预报的b系下的速度的差值、
Figure GDA0002930001960000055
表示惯导预报的ECEF系相对于b系的旋转矩阵、
Figure GDA0002930001960000056
表示惯导预报的ECEF系下的速度、lb表示里程计与惯导的杆臂、
Figure GDA0002930001960000057
表示地球自转角速度。
进一步地，本发明的步骤6中融合GNSS/车道线约束/里程计融合后，得到的观测方程为：
Figure GDA0002930001960000058
其中，vP和vL表示载波和相位观测值，
Figure GDA0002930001960000059
表示站间星间双差观测值的方向余弦向量，
Figure GDA00029300019600000510
表示天线与惯导在ECEF系下的杆臂，δN为双差模糊度状态参数，εP、εL、εf、εd、
Figure GDA00029300019600000511
分表表示载波、相位、车道线侧向观测值、车道线高程观测值以及里程计观测值的观测噪声。
进一步地，本发明的步骤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紧组合状态模型和观测模型，分别如下：
Figure GDA0002930001960000071
δz＝HδX+η (2)
式(1)中，δXSINS＝(δre δve φ ab εb)T，
Figure GDA0002930001960000072
分别为SINS和GNSS的状态参数，由于采用了双差的定位模式，GNSS接收机钟差已被消去，只留下了站间差分模糊度。F为状态微分方程系数矩阵，w为过程噪声；式(2)中，δz为观测值残差，H为设计矩阵，η为观测噪声。
按照上述紧组合数学模型，采用Kalman滤波进行估计，其一般解算步骤包括一步状态预测和观测更新两部分，具体如下：
一步状态预测：
Figure GDA0002930001960000073
观测更新：
Figure GDA0002930001960000074
式(3)中，Xk,k-1为预报状态，Φk,k-1为状态转移矩阵，Qk-1为过程噪声，Δtk为两个历元间的时间间隔，Pk,k-1为预报方差。式(4)中，Kk为增益系数，Rk为观测噪声，Xk和Pk为滤波状态及其方差。
以上紧组合数学模型和Kalman滤波一般解算步骤是本发明算法的基础，下面将结合图1所示的技术路线，对本发明中各模块的关键技术及其实施方法展开详细叙述。
一、基于卡尔曼滤波的车道线提取
车道线提取方法可分基于道路特征和道路模型的车道线提取方法和基于深度学习的车道线提取方法两大类。深度学习具有较好的检测效果，但依赖大量的样本数据进行模型训练。当前，我们仍以传统的方法为主，利用车道线特征、颜色、结构等区别于道路背景的信息，来分离得到车道线，并采用滑动窗口的卡尔曼滤波方法进行改进，流程如图3所示，主要包括预处理、车道线提取和车道线跟踪三大模块。
1)预处理
在采集数据前，通过Matlab标定工具箱对单目相机进行单目标定，获得相机内参数，包括焦距fx、fy、像主点位移cx、cy以及相机的径向畸变系数和切向畸变系数。预处理时，利用上述参数对单目相机采集的车道线图像进行畸变校正。根据单目相机安装的位置和视野范围，划定感兴趣区域，排除其他无关区域干扰，并利用单应性变换公式(5)，将车道线恢复到俯视状态下，H为对应的单应性矩阵，u,v为像素坐标系下的坐标，[X,Y,Z]为俯视状态下的对应点坐标，这样可便于后续车道线提取处理。
Figure GDA0002930001960000081
2)车道线提取
输入车道线正射影像，通过颜色和边缘信息的阈值分割，将车道线正射影像二值化。由于车道线通常为白色和黄色，且车道线为引导驾驶员开车，通常对光反射强度较高，颜色较亮，故综合使用LAB颜色空间的B通道、HSL和HSV颜色空间的亮度通道检测车道线，以将车道线和背景进行分离。二值化后，对图像底部划分像素窗口，并对窗口灰度值进行从左到右的直方图统计，然后采用高斯核平滑得到灰度均值曲线图，提取波峰位置作为车道线位置。
3)车道线跟踪
由于车道线在车辆行驶过程中是连续缓慢变化的，根据这一性质，可采用卡尔曼滤波对车道线进行动态、连续、平滑跟踪。
使用速度模型建立前后历元的关系，状态方程如下，式中，qx和qv分别是位置x和速度v的过程噪声：
Figure GDA0002930001960000091
观测方程如下，yk+1为当前提取到的车道线位置：
yk+1＝xk+1+ε (7)
滤波后，得到每个窗口i的位置
Figure GDA0002930001960000092
最后再将连续窗口的位置进行二次曲线拟合，从而得到全时段连续平滑的车道线提取，实现对车道线虚线、斑马线的平滑过度。
成功提取车道线后，将获得的底部左右车道线在相机坐标系下的x坐标，即相机和左右车道线的距离lL和lR，反馈给GNSS/INS滤波器。
二、八叉树辅助的车道线快速搜索
车道线地图包含车道的全部信息，包括坐标、航向、曲率、颜色、拓扑关系等等。为提高车道线搜索效率，在对车道线地图数据存储时，采用八叉树结构，在坐标域进行组织管理，对点云坐标建立空间索引，实现车道线坐标的空间聚类、划分。八叉树的每个结点表示一个正方体的体积元素，父结点可分为八个子节点，如此分割下去，直到不可分割。存储车道线地图时，将空间八叉树最小体素的大小设置为10m，那么对于40km长的车道来说，最多也只需12次搜索就可知道车道线坐标位于哪个体素。
1)最邻近车道线节点搜索
根据惯导预报位置，如图5所示的点
Figure GDA0002930001960000093
从车道线的空间八叉树存储结构寻找预报点对应的子节点体素。找到体素后，遍历体素中的所有车道线坐标点，根据欧式几何距离法找到距离最邻近的点。
2)一致性判断
根据惯导预报的位置点的姿态中的航向信息得到载体的前进方向，通过前进方向和搜索到的车道线节点的航向进行一致性判断，若两者航向之差大于阈值，则表明搜索到的车道线节点有误。
根据历史时刻成功搜索到的车道线节点和当前搜索到的车道线节点的拓扑关系检验，判断历史节点和当前节点是否具备邻接拓扑关系，若不具备，则表明搜索到的车道线节点有误。
若通过上述一致性判断，则表明搜索到的车道线节点正确。由于载体实际上所处的车道线是左右两段连续的曲线，而目前搜索到的仅为最邻近的左右两个车道线节点，故首先需根据节点的曲率信息，将节点向前向后外推n个节点，n取决于节点曲率的大小，曲率大则n也可适当放大。外推后可得到左车道线的2个节点PL1、PL2以及右车道线的2个节点PR1、PR2，如图5所示。
三、车道线辅助GNSS/INS高程定位约束
已经得到了最邻近的左车道线节点PL1、PL2和右车道线的2个节点PR1、PR2，理论上，如果惯导预报位置没有误差，则其中心经过车高和与相机的外参改正后应位于车道面上，由此可建立高程约束。
假设惯导中心到地面的距离事先已知，设为dI，左右车道线的四点坐标也已知，为(PL1,PL2,PR1,PR2)，地平面可表示为A·X+B·Y+C·Z+1＝0，将车道线的四点坐标代入，求得平面系数(A,B,C)，根据点到面的距离公式，可得惯导中心到地平面的距离为：
Figure GDA0002930001960000101
因为车辆一定在路面上方，所以式中的绝对值符号可以去掉，最终表示为惯导位置误差的观测方程：
Figure GDA0002930001960000102
式中，
Figure GDA0002930001960000103
为预报距离，将惯导位置的预报值代入(8)式得到，Hlane为高程约束的系数矩阵，表示如下：
Figure GDA0002930001960000104
最后形成的观测方程如下：
Figure GDA0002930001960000111
dI为常值，如果事先没有测量出来，也可以实时计算。在GNSS信号良好的情况下，GNSS/SINS组合可以得到厘米级的定位结果，此时直接计算惯导中心到地面的垂直距离，保存以后为后续高程约束所用。
四、车道线辅助GNSS/INS侧向定位约束
侧向约束方程需要通过相机中心做为媒介，通过车道线提取、单应性变换，我们得到了相机中心到车道线的水平距离，左右车道线的距离分别记为dL和dR。假设相机中心的位置为Pc，它到路面存在一个高度值，因此，要计算相机到车道线的水平距离，相机中心必须和路面在同一个平面上。
为统一计算，需再构建观测方程时，将路面抬高到相机中心处，具体做法如下：1)获得相机中心到路面的距离dc。步骤4中，我们已经得到了惯导中心到路面的距离，而惯导与相机的空间又是在采集数据之前提前标定好的，属于已知。因此，相机中心到路面的距离
Figure GDA0002930001960000112
Figure GDA0002930001960000113
为两者的垂向距离；2)将路面坐标(PL1,PL2,PR1,PR2)抬高，由于地平面系数
Figure GDA0002930001960000114
即为该平面的单位法向量，因此，路面坐标加上同一个向量增量
Figure GDA0002930001960000115
得到新的路面坐标(P′L1,P′L2,P′R1,P′R2)即为抬升至相机中心的车道线。
此时，通过构造两个向量，叉乘得到两个向量围成的四平行面积，然后除以底边，计算得到高，即为相机中心到车道线的水平距离：
Figure GDA0002930001960000116
(Pc-P′L1)×mL向量叉乘展开表示为：
Figure GDA0002930001960000121
对(12)式两边距离平方后做为新观测量，使用MATLAB代数工具箱，得到平方距离fL＝(dL·||mL||)2对Pc的偏导数：
Figure GDA0002930001960000122
式中，M1、M2和M3表示如下：
Figure GDA0002930001960000123
将相机中心坐标Pc和惯导中心坐标PSINS关系为：
Figure GDA0002930001960000124
式中，
Figure GDA0002930001960000125
lb是相机中心在惯导坐标系下的杆臂分量。由此得到Pc对惯导位姿误差的偏导数为：
Figure GDA0002930001960000126
式中，re＝PSINS，为了与状态方程一致。根据链式法则，由(14)式和(17)式，得到平方距离f对惯导位姿误差的偏导数：
Figure GDA0002930001960000127
最后，得到观测方程为：
Figure GDA0002930001960000131
五、里程计辅助GNSS/INS前向定位约束
采用轮式里程计进行前向约束有两个假设：1)运动载体没有侧滑，沿载体坐标系前进方向运动，即侧向速度为零；2)载体贴着地表运动，没有上下波动，即垂向速度为零。
由里程计得到的前向速度为：
Figure GDA0002930001960000132
式中，
Figure GDA0002930001960000133
是里程计速度转到惯导本体系下的速度，
Figure GDA0002930001960000134
是里程计速度，只在前向有速度，其余两轴为零，
Figure GDA0002930001960000135
lb是里程计中心在惯导本体系下的位置矢量，即里程计杆臂改正。
从该公式出发对里程计约束的观测模型进行推导，位置和速度状态均以SINS中心为基准。采用扰动法，可得：
Figure GDA0002930001960000136
此时，推导所得的观测方程如下：
Figure GDA0002930001960000137
以上为严格推导的观测模型，但可根据实际情况用以下模型进行简化。
当车辆直线行驶时，惯导系没有相对于地面转动，
Figure GDA0002930001960000138
接近于零，此时，里程计速度等于惯导系下的前进速度。当车辆转弯时，
Figure GDA0002930001960000141
计算得到的
Figure GDA0002930001960000142
会在测向(X轴)出现速度分量，与非完整性约束假设不一致，这是因为车辆转弯时以后轮为定轴转动，不在后轮轴面上的点都会出现转动速度，但实际中地面车辆转弯比较缓慢，该速度分量很小，稍微放大点观测噪声即可，可直接认为vSINS＝vDMI。此时观测方程可简化成：
Figure GDA0002930001960000143
式中，
Figure GDA0002930001960000144
和
Figure GDA0002930001960000145
为当前时刻机械编排得到的值，下标2表示取第二行为：
Figure GDA0002930001960000146
六、多源融合观测更新
上述步骤中，我们已经得到了车道线高程、侧向约束的方程以及里程计的前向约束观测方程。整个融合定位系统以差分GNSS/SINS紧组合为基础，动态的加入车道线约束观测方程和里程计观测方程，如图6所示。
当GNSS卫星数较多以及PDOP值较小时，说明GNSS定位几何条件满足要求，此时，可以删去车道线和里程计的观测，状态方程仍然不变。
当GNSS卫星数较少时并且车道线检测成功时，向观测方程中加入车道线的侧向和高程约束方程。
在道路两侧高楼林立的城市场景下，GNSS信号遮挡严重，但道路前后向遮挡较少，GNSS卫星仍可见，此时，选择载体上方，即高度角最高的卫星作为基准星，将基准星和道路前后向的可见卫星组成双差观测值参与定位解算，并同时加入里程计的前向约束。当车辆处于高架桥下，隧道等场景中，GNSS卫星将全部不可见，此时前向只采用里程计误差约束。
完整建立GNSS/车道线约束/里程计融合的观测方程如下：
Figure GDA0002930001960000151
vP和vL表示载波和相位观测值，ε为各观测值噪声。
应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (8)

1.一种GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，该方法中，通过视觉传感器采集图像，离线生成车道线地图数据库；在载体进行实时解算导航定位时，以差分GNSS/INS紧组合为基础，GNSS遮挡时，通过载体上安装的视觉传感器探测载体和车道线的相对关系，在车道线地图的辅助下，动态加入车道线约束观测方程辅助侧向、高程定位；并利用载体上方和道路前进方向的可观测卫星形成差分观测值参与紧组合，约束载体前向误差发散；该方法具体包括以下步骤：
步骤1、对实时采集的影像预处理得到车道线影像的俯视影像，对影像进行颜色空间转换分离出车道线，并基于滑动窗口的灰度直方图统计确定车道线的坐标，最后利用基于匀速假设的卡尔曼滤波对车道线进行跟踪，得到连续平滑的相机中心距离左右影像的距离；
步骤2、利用八叉树数据结构快速搜索到惯导预报位置的最小体素，利用欧氏几何距离法找到最邻近车道线节点，并对节点进行航向/拓扑一致性检验，检验通过后根据曲率返回最佳匹配的PL1、PL2、PR1、PR2；
步骤3、根据惯导预报位置，经过车高和与相机的外参改正后，位于车道面上的假设，构建车道线高程约束观测方程；
步骤4、将PL1、PL2、PR1、PR2四个点抬高至相机中心处，根据相机中心预报值对车道线的距离应等于观测距离的假设构建相机中心与车道线的侧向约束观测方程，并利用链式法则，将其转换至惯导中心；
步骤5、根据载体在直线运动时打滑较少且转弯时速度较慢的假设，构建里程计前向约束的观测方程；
步骤6、根据车道线高程、侧向约束的方程以及里程计的前向约束观测方程，实现多源融合观测更新。
2.根据权利要求1所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤1中进行车道线跟踪的方法具体为：
步骤1.1、预处理：通过Matlab标定工具箱对单目相机进行单目标定，获得相机内参数，利用相机内参数对单目相机采集的车道线图像进行畸变校正；根据单目相机安装的位置和视野范围，划定感兴趣区域，排除其他无关区域干扰，并利用单应性变换，将车道线恢复到俯视状态下；
步骤1.2、车道线提取：输入车道线正射影像，通过颜色和边缘信息的阈值分割，将车道线正射影像二值化；对图像底部划分像素窗口，并对窗口灰度值进行从左到右的直方图统计，采用高斯核平滑得到灰度均值曲线图，提取波峰位置作为车道线位置；
步骤1.3、车道线跟踪：采用卡尔曼滤波对车道线进行跟踪，使用速度模型建立前后历元的关系，状态方程为：
Figure FDA0002930001950000021
其中，qx和qv分别是位置x和速度v的过程噪声，k为时刻；
观测方程为：
yk+1＝xk+1+ε
其中，yk+1为当前提取到的车道线位置；ε表示提取车道线位置的观测噪声；
滤波后，得到每个窗口i的位置
Figure FDA0002930001950000022
再将连续窗口的位置进行二次曲线拟合，从而得到全时段连续平滑的车道线提取，实现对车道线虚线、斑马线的平滑过度；成功提取车道线后，将获得底部左右车道线在相机坐标系下的x坐标，即相机和左右车道线的距离。
3.根据权利要求2所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤2中八叉树辅助的车道线快速搜索的具体方法为：
步骤2.1、最邻近车道线节点搜索：根据惯导预报位置，从车道线的空间八叉树存储结构寻找预报点对应的子节点体素；找到体素后，遍历体素中的所有车道线坐标点，根据欧式几何距离法找到距离最邻近的点；
步骤2.2、一致性判断：根据惯导预报的位置点的姿态中的航向信息得到载体的前进方向，通过前进方向和搜索到的车道线节点的航向进行一致性判断，若两者航向之差大于阈值，则表明搜索到的车道线节点有误；
根据历史时刻成功搜索到的车道线节点和当前搜索到的车道线节点的拓扑关系检验，判断历史节点和当前节点是否具备邻接拓扑关系，若不具备，则表明搜索到的车道线节点有误；
反之，表明搜索到的车道线节点正确。
4.根据权利要求1所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤3中构建的车道线高程约束观测方程为：
Figure FDA0002930001950000031
其中，
Figure FDA0002930001950000032
为预报距离，dI为常值，Hlane为高程约束的系数矩阵，δre、δve、φ、ab、εb分别表示位置误差、速度误差、姿态误差以及加速度计零偏、陀螺零偏；高程约束的系数矩阵表示为：
Figure FDA0002930001950000033
其中，A,B,C为平面系数。
5.根据权利要求2所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤4中构建相机中心与车道线的侧向约束观测方程为：
Figure FDA0002930001950000034
其中，fL、fR表示观测到的离左右车道线的距离，
Figure FDA0002930001950000035
表示预报位置距离搜索到的左右车道线的距离，
Figure FDA0002930001950000036
表示观测值对位置、姿态的偏导数。
6.根据权利要求5所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤5中构建的里程计前向约束的观测方程为：
Figure FDA0002930001950000041
其中，δvb表示里程计观测值和惯导预报的b系下的速度的差值、
Figure FDA0002930001950000042
表示惯导预报的ECEF系相对于b系的旋转矩阵、
Figure FDA0002930001950000043
表示惯导预报的ECEF系下的速度、lb表示里程计与惯导的杆臂、
Figure FDA0002930001950000044
表示地球自转角速度。
7.根据权利要求6所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤6中GNSS/车道线约束/里程计融合后，得到的观测方程为：
Figure FDA0002930001950000045
其中，vP和vL表示载波和相位观测值，
Figure FDA0002930001950000046
表示站间星间双差观测值的方向余弦向量，
Figure FDA0002930001950000047
表示天线与惯导在ECEF系下的杆臂，δN为双差模糊度状态参数，εP、εL、εf、εd、
Figure FDA0002930001950000048
分别表示载波的观测噪声、相位的观测噪声、车道线侧向观测值的观测噪声、车道线高程观测值的观测噪声以及里程计观测值的观测噪声。
8.根据权利要求1所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤6中进行多源融合的方法具体为：
动态地选择是否加入车道线、里程计约束，在GNSS满足观测条件时，仅用GNSS/INS进行定位，在GNSS受到遮挡或中断时，加入里程计约束，并根据车道线探测情况决定是否加入车道线约束。
CN201910659041.4A 2019-07-22 2019-07-22 一种gnss/惯导/车道线约束/里程计多源融合方法 Active CN110411462B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910659041.4A CN110411462B (zh) 2019-07-22 2019-07-22 一种gnss/惯导/车道线约束/里程计多源融合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910659041.4A CN110411462B (zh) 2019-07-22 2019-07-22 一种gnss/惯导/车道线约束/里程计多源融合方法

Publications (2)

Publication Number Publication Date
CN110411462A CN110411462A (zh) 2019-11-05
CN110411462B true CN110411462B (zh) 2021-05-18

Family

ID=68362292

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910659041.4A Active CN110411462B (zh) 2019-07-22 2019-07-22 一种gnss/惯导/车道线约束/里程计多源融合方法

Country Status (1)

Country Link
CN (1) CN110411462B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110866482B (zh) * 2019-11-08 2022-09-16 广东工业大学 一种里程计数据源动态选择方法、装置和设备
CN110888440A (zh) * 2019-11-28 2020-03-17 山东三木环保工程有限公司 一种结合gnss卫星定位和遮挡板的有轨车辆对门系统及方法
CN112945266B (zh) * 2019-12-10 2024-07-19 炬星科技（深圳）有限公司 激光导航机器人及其机器人的里程计校准方法
CN111026081B (zh) * 2019-12-10 2021-05-11 苏州智加科技有限公司 一种误差计算方法、装置、设备及存储介质
CN111274343B (zh) * 2020-01-20 2023-11-24 阿波罗智能技术(北京)有限公司 一种车辆定位方法、装置、电子设备及存储介质
CN111380516B (zh) * 2020-02-27 2022-04-08 上海交通大学 基于里程计测量信息的惯导/里程计车辆组合导航方法及系统
CN111272165B (zh) * 2020-02-27 2020-10-30 清华大学 一种基于特征点标定的智能车定位方法
CN112581795B (zh) * 2020-12-16 2022-04-29 东南大学 一种基于视频的船桥及船间碰撞实时预警方法与系统
CN112596089B (zh) * 2021-03-02 2021-05-18 腾讯科技（深圳）有限公司 融合定位方法、装置、电子设备及存储介质
CN114136315B (zh) * 2021-11-30 2024-04-16 山东天星北斗信息科技有限公司 一种基于单目视觉辅助惯性组合导航方法及系统
CN114646992B (zh) * 2022-03-21 2024-08-02 腾讯科技（深圳）有限公司 定位方法、装置、计算机设备、存储介质和计算机程序产品
TWI814480B (zh) * 2022-07-11 2023-09-01 新馳科技股份有限公司 車輛定位系統與車輛定位方法
CN116129389B (zh) * 2023-03-27 2023-07-21 浙江零跑科技股份有限公司 一种车道线获取方法、计算机设备、可读存储介质及机动车
CN116540286B (zh) * 2023-07-06 2023-08-29 中国科学院空天信息创新研究院 基于非完整性约束的多源鲁棒融合定位方法
CN116642501B (zh) * 2023-07-25 2023-09-29 齐鲁空天信息研究院 一种惯性为核心的车道线辅助定位的多源融合方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107229063A (zh) * 2017-06-26 2017-10-03 奇瑞汽车股份有限公司 一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法
CN107728175A (zh) * 2017-09-26 2018-02-23 南京航空航天大学 基于gnss和vo融合的无人驾驶车辆导航定位精度矫正方法
US10151588B1 (en) * 2016-09-28 2018-12-11 Near Earth Autonomy, Inc. Determining position and orientation for aerial vehicle in GNSS-denied situations
CN109405824A (zh) * 2018-09-05 2019-03-01 武汉契友科技股份有限公司 一种适用于智能网联汽车的多源感知定位系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10151588B1 (en) * 2016-09-28 2018-12-11 Near Earth Autonomy, Inc. Determining position and orientation for aerial vehicle in GNSS-denied situations
CN107229063A (zh) * 2017-06-26 2017-10-03 奇瑞汽车股份有限公司 一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法
CN107728175A (zh) * 2017-09-26 2018-02-23 南京航空航天大学 基于gnss和vo融合的无人驾驶车辆导航定位精度矫正方法
CN109405824A (zh) * 2018-09-05 2019-03-01 武汉契友科技股份有限公司 一种适用于智能网联汽车的多源感知定位系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
融合视觉的智能车组合导航技术分析;曾庆喜等;《导航定位学报》;20170630;第2.2-2.3节 *

Also Published As

Publication number Publication date
CN110411462A (zh) 2019-11-05

Similar Documents

Publication Publication Date Title
CN110411462B (zh) 一种gnss/惯导/车道线约束/里程计多源融合方法
CN111551958B (zh) 一种面向矿区无人驾驶的高精地图制作方法
US11002859B1 (en) Intelligent vehicle positioning method based on feature point calibration
Ding et al. LiDAR inertial odometry aided robust LiDAR localization system in changing city scenes
CN110412635B (zh) 一种环境信标支持下的gnss/sins/视觉紧组合方法
Rose et al. An integrated vehicle navigation system utilizing lane-detection and lateral position estimation systems in difficult environments for GPS
CN107782321B (zh) 一种基于视觉和高精度地图车道线约束的组合导航方法
JP2022019642A (ja) マルチセンサ融合に基づく測位方法及び装置
CN114199240B (zh) 无gps信号下二维码、激光雷达与imu融合定位系统及方法
CN114199259B (zh) 一种基于运动状态与环境感知的多源融合导航定位方法
Tao et al. Lane marking aided vehicle localization
JP5162849B2 (ja) 不動点位置記録装置
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN113654555A (zh) 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
CN107246868A (zh) 一种协同导航定位系统及导航定位方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN107229063A (zh) 一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法
CN115407357A (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
Xiao et al. LIO-vehicle: A tightly-coupled vehicle dynamics extension of LiDAR inertial odometry
Wang et al. Simultaneous localization of rail vehicles and mapping of surroundings with LiDAR-inertial-GNSS integration
Deusch et al. Improving localization in digital maps with grid maps
CN116929363A (zh) 一种基于可通行地图的矿用车辆自主导航方法
CN113777589B (zh) 一种基于点特征的lidar与gps/imu联合标定方法
Huang et al. Wheel odometry aided visual-inertial odometry for land vehicle navigation in winter urban environments

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant