一种惯性为核心的车道线辅助定位的多源融合方法

1.本发明属于多源传感器融合定位技术领域，具体涉及一种惯性（imu，inertial measurement unit)为核心的车道线辅助定位的多源融合方法。


背景技术：

2.近年来，智能驾驶、无人导航定位等新兴技术的快速发展，其对于高精度定位的需求也越来越高，任何单一的传感器已经无法满足复杂环境下的定位，实现高精度、高可靠的定位是只能驾驶发展的关键，因此，多源传感器融合已成为工业界关注的热点。
3.惯性导航系统ins (inertial navigation system, ins) 因其航迹推算的定位方式，由于积分的影响，误差会随着时间的延续而积累，造成定位结果的发散。目前成熟的算法gnss/ins组合定位的方式，可以对ins误差进行补偿，而ins预测信息也能辅助gnss的模糊度固定。然而在城市环境下（楼群、高架桥、隧道等）gnss信号受干扰比较严重，可能导致卫星信号长时间失锁或者定位异常，ins的误差也不能得到补偿，定位精度大大下降，因此需要其他传感器的辅助定位。视觉传感器定位能在一定程度上缓解gnss长时间不可用的情况，作为一种匹配定位的方式，其定位精度受到环境光照强度、纹理强弱等影响，从而特征点的误匹配，影响定位精度，在应用过程中也会受到一定程度上的限制。
4.目前，高精度地图辅助gnss/ins/视觉定位基本采用的地图匹配的方法识别车道线，地图匹配主要分为点-点、点-线、线-线三种匹配方式，点-点匹配依赖于节点的数量，需要大量的定位节点，点-线匹配算法需要先识别出一些线段，然后进行搜索最短线段，线-线匹配方式精度较高，但算法复杂，实时性较差，且相机识别的车道线与高精度地图匹配融合之后得到的定位结果，在融合的过程中，地图匹配中的误差也会引入。在gnss挑战环境下，如城市峡谷，由于gnss卫星分布不均匀，可能只分布于某一方向，导致gnss/ins/视觉集成定位结果的可靠性较差，另一方面，由于特征点的分布也是视觉定位的关键，在一些环境下，特征点的几何分布结构较差，也会导致最终定位结果不准确。因此，在gnss/ins/视觉集成定位时，需要综合考虑到gnss卫星的分布与有效卫星数和特征点的分布情况。


技术实现要素：

5.为了解决上述问题，本发明提出一种惯性为核心的车道线辅助定位的多源融合方法。一方面，对gnss的状态进行检核，处于失锁或定位异常状态均不参与结算；另外，在城市复杂环境下，gnss卫星与特征点的分布也会影响定位的精度，本发明引入性能指标来衡量卫星的可用性和特征点几何分布的优度，并将其作为是否将该传感器测量值添加到滤波器的标准，实现选择性集成定位，系统具有灵活性；另一方面，引入高精度地图，通过识别车辆运行时两侧的车道线，并计算车辆到左侧车道线的距离，将该距离与相机预测的车辆到车道线距离做差，作为一种侧向约束，带入到msckf滤波中，提高系统的定位精度。
6.本发明实现其目的所采取的技术方案是：一种惯性为核心的车道线辅助定位的多源融合方法，包括如下步骤：
步骤（1）对于信号失锁的情况，通过检测一段时间内卫星颗数，在时间长度k内，若有效卫星颗数持续少于4颗，则gnss信号不参与解算；步骤（2）对于定位异常的情况，检测一段时间内卫星分布的空间几何强度因子pdop与有效卫星颗数的跳变情况；步骤（3）引入高精度地图，通过高精度地图识别车辆所在的车道线信息，进而测量出车辆距离车道线的距离；步骤（4）使用单目相机通过高斯滤波平滑、边缘点检测提取车道线轮廓信息，通过多项式建模建立车道线模型，通过车道线轮廓信息和车道线模型分别计算出车辆到车道线之间的距离；步骤（5）将步骤（4）中通过车道线轮廓信息和车道线建模分别计算出的车辆到车道线之间的距离做差并带入滤波中优化解算载体位置信息；步骤（6）以惯性为核心，引入性能指标以选择性集成多传感器，当有效卫星颗数不足或者测量特征点数量较少时，通过性能指标识别与判断。
7.本发明的有益效果在于：针对gnss/ins/视觉多源传感器融合定位，以ins为核心，首先对于gnss的卫星颗数及定位状态进行检测，然后根据性能指标来判断gnss与视觉是否参与集成解算，定位方式灵活。加入高精度地图的辅助，将相机识别到的车道线距离与高精度地图计算得到的车道线距离，将两者直接做差来提供横约束加入到卡尔曼滤波中，有效抑制误差的发散且不会引入地图匹配的误差，提供了连续、可靠的定位结果。
附图说明
8.图1为本发明的一种惯性为核心的车道线辅助定位的多源融合方法流程示意图。
具体实施方式
9.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
10.本发明引入性能指标来衡量卫星的可用性和特征点几何分布的优度，并将其作为参与优化的准则；引入高精度地图，将其识别的车道线距离与相机预测的车辆到车道线距离作为侧向约束，带入到msckf滤波中，提高系统的定位精度。
11.城市环境下gnss信号可能受遮挡严重，会出现信号失锁或定位异常，此时添加判断来识别是否参与解算。信号失锁是指gnss接收机接收不到信号或者有效卫星数少于4颗，定位异常是指由于信号的遮挡导致卫星的可见卫星数变化较大，卫星分布的空间几何强度因子pdop (position dilution of precision, pdop) 值也变化较大。
12.如图1所示，本发明的一种惯性为核心的车道线辅助定位的多源融合方法具体包括如下步骤：步骤a) 对于信号失锁的情况，可通过检测一段时间内卫星颗数，认为在时间段k内，如果有效卫星颗数持续少于4颗，则gnss信号不参与解算。
unit, imu）坐标系下的位置，需要用过杆臂补偿的方式归算得到p点的位置。
17.(6)式中，表示p点在导航坐标系n系的位置信息，表示惯性导航系统估计得到的车辆在导航坐标系n系下的位置信息，表示从载体坐标系b系转换到导航坐标系n系的方向余弦矩阵，表示相机与惯性导航系统之间的杆臂。
18.定义相机坐标系以车辆前进方向为x轴，y轴垂直于x轴指向左为正，惯性导航系统坐标系x轴与y轴均与相机坐标系平行，z轴垂直于xoy平面构成右手坐标系。由于车道线只是对于载体的横向误差进行约束，所以只考虑横向车辆到车道线的距离，且假设车道线为直线。
19.步骤d）单目相机可以通过高斯滤波平滑、边缘点检测等识别出轮廓信息，进一步多项式建模得到车道线，也可以计算出车辆到车道线之间的距离。将两种方式计算得到的车辆到车道线之间的距离做差，代入到msckf滤波中进行优化解算，从而得到车辆位置信息。
20.相机探测识别车道线主要分为两大部分，轮廓提取与建模。车道线轮廓提取主要是计算机视觉相关内容，包括相片的高斯滤波平滑、边缘检测、轮廓信息的生成及过滤非轮廓信息等步骤。车道线建模主要是采用霍夫变换及曲线拟合的方式建立车道线模型。根据相机坐标系及车道线函数模型即可表示出车道线与相机之间的关系。
21.步骤e) 高精度地图识别得到的车道线距离与相机识别检测得到的车道线距离之差作为观测值，可得：(7)式中，、分别表示地图得到的车线距离观测量与相机识别检测得到的车道线距离。可将该式加入到卡尔曼滤波的观测方程中形成约束。
22.步骤f）以惯性导航系统为核心，引入性能指标来选择性集成多传感器，假设卡尔曼滤波（msckf）总的观测方程为：(8)式中，表示观测向量，表示系数矩阵，表示状态向量，表示测量噪声。假设每个gnss测量值与相机的测量值之间独立，权重函数一般与协方差矩阵成反比，可以定义为：(9)
(10)式中，w为权重，表示尺度常数用来定义权重，表示gnss的误差协方差，表示特征点的误差协方差，表示的协方差矩阵，的协方差矩阵可以写成：(11)(12)式中，表示状态向量，h表示系数矩阵。
23.性能指标wdop记为：(13)式中，trace表示求迹符号。
24.由于卫星的分布以及特征点的几何分布会影响最终的定位性能，当特征点分布在较窄的区域或卫星在水平方向与特征点分布方向一致时，会导致定位结果不准确，因此引入性能指标来选择性集成解算，当有效卫星颗数不足或者测量特征点数量较少的情况会被性能指标wdop识别与判断。
25.本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种惯性为核心的车道线辅助定位的多源融合方法，其特征在于，包括如下步骤：步骤（1）对于信号失锁的情况，通过检测一段时间内卫星颗数，在时间长度k内，若有效卫星颗数持续少于4颗，则gnss信号不参与解算；步骤（2）对于定位异常的情况，检测一段时间内卫星分布的空间几何强度因子pdop与有效卫星颗数的跳变情况；步骤（3）引入高精度地图，通过高精度地图识别车辆所在的车道线信息，进而测量出车辆距离车道线的距离；步骤（4）使用单目相机通过高斯滤波平滑、边缘点检测提取车道线轮廓信息，通过多项式建模建立车道线模型，通过车道线轮廓信息和车道线模型分别计算出车辆到车道线之间的距离；步骤（5）将步骤（4）中通过车道线轮廓信息和车道线建模分别计算出的车辆到车道线之间的距离做差并带入滤波中优化解算载体位置信息；步骤（6）以惯性为核心，引入性能指标以选择性集成多传感器，当有效卫星颗数不足或者测量特征点数量不足时，通过性能指标识别与判断。2.根据权利要求1所述的一种惯性为核心的车道线辅助定位的多源融合方法，其特征在于，所述步骤（2）包括：记相邻两个历元之间的卫星颗数变化之差和空间几何强度因子pdop变化之差为：(2)(3)式中，、分别表示i历元和i-1历元的卫星颗数，、分别表示i历元和i-1历元的卫星的空间几何强度因子pdop值，、分别表示相邻两个历元之间的卫星颗数之差与空间几何强度因子pdop之差；时间长度k内，、超过阈值与时间长度k之间的比值记为：(4)(5)式中，、为设定的阈值，表示k时间段内相邻两个历元卫星颗数之差大于的次数，表示k时间段内相邻两个历元的空间几何强度因子pdop值之差大于的次数；当或大于阈值时，认为
gnss定位状态异常，gnss信号不参与解算。3.根据权利要求2所述的一种惯性为核心的车道线辅助定位的多源融合方法，其特征在于，所述步骤（3）包括：通过惯性导航系统预测的位置信息，识别到当前车辆最近的车道线，然后将当前车辆的位置投影于车道线上，计算得到当前车辆与车道线之间的距离；假设p点为距离当前车辆最近的相机正前方的点，则通过惯性导航系统机械编排估计得到的p点的位置信息得到高精度地图中的车道线信息，利用点到直线关系，p点投影到车道线得到，即得到p点到车道线的距离。4.根据权利要求3所述的一种惯性为核心的车道线辅助定位的多源融合方法，其特征在于，所述步骤（4）中，提取车道线轮廓信息包括对单目相机获得的相片进行高斯滤波平滑、边缘检测、轮廓信息的生成及过滤非轮廓信息；采用霍夫变换及曲线拟合的方式建立车道线模型。5.根据权利要求4所述的一种惯性为核心的车道线辅助定位的多源融合方法，其特征在于，所述步骤（5）包括：高精度地图识别得到的车道线距离与单目相机识别检测得到的车道线距离之差作为观测值，得到：(7)式中，、分别表示高精度地图得到的车道线距离与单目相机识别检测得到的车道线距离；将式（7）加入到卡尔曼滤波的观测方程中形成约束。6.根据权利要求5所述的一种惯性为核心的车道线辅助定位的多源融合方法，其特征在于，所述步骤（6）包括：假设卡尔曼滤波总的观测方程为：(8)式中，表示观测向量，表示系数矩阵，表示状态向量，表示测量噪声；假设每个gnss测量值与单目相机的测量值之间独立，权重函数与协方差矩阵成反比，定义为：(9)(10)式中，w为权重，表示尺度常数用来定义权重，表示gnss的误差协方差，表示特征点的误差协方差，表示的协方差矩阵，的协方差矩阵写成：
(11)(12)式中：表示状态向量，h表示系数矩阵；e[]表示状态函数；性能指标wdop记为：(13)式中，trace表示求迹符号。

技术总结
本发明公开了一种惯性为核心的车道线辅助定位的多源融合方法，属于多源传感器融合定位技术领域，引入性能指标来衡量卫星的可用性和特征点几何分布的优度，并将其作为参与优化的准则；引入高精度地图，将其识别的车道线距离与相机预测的车辆到车道线距离作为侧向约束，带入到多状态约束下的卡尔曼滤波器MSCKF(Multi-State Constraint Kalman Filter)中，提高系统的定位精度。本发明有效抑制误差的发散且不会引入地图匹配的误差，提供了连续、可靠的定位结果。靠的定位结果。靠的定位结果。


技术研发人员：李子申 蔚科 王亮亮 刘炳成 刘振耀 常坤 楚焕鑫 谢吉顺
受保护的技术使用者：中国科学院空天信息创新研究院
技术研发日：2023.07.25
技术公布日：2023/8/28