一种地图辅助的多传感器融合定位方法及计算机可读介质

1.本发明属于导航定位技术领域，尤其涉及一种地图辅助的多传感器融合定位方法及计算机可读介质。


背景技术：

2.近年来，智能驾驶技术越来越受到人们的关注，高精度定位技术在其中起关键作用。智能驾驶定位的目的是估计智能载体相对于地图或者道路的位置和姿态。为确保智能载体在移动过程中的安全性，实时的、高精度的导航定位信息至关重要。
3.开阔场景中的高精度定位已经不再困难，使用gnss实时动态载波相位差分(gnss real-time kinematic，gnss-rtk)技术进行定位，在整周模糊度固定的情况下可以达到厘米级定位精度。在gnss退化场景中，可用卫星颗数较少，gnss提供的定位信息不再可靠。如何在gnss退化场景中实现连续可靠的车道级定位是目前研究的热点。


技术实现要素：

4.为了解决上述技术问题，本发明针对gnss退化场景下的低成本车道级实时定位的应用需求，提出了一种地图辅助的多传感器融合定位方法及计算机可读介质。
5.本发明的技术方案为一种地图辅助的多传感器融合定位方法，具体步骤如下：
6.步骤1：获取高精度地图数据；
7.步骤2：将高精度地图数据在lanelets地图框架下，结合每条虚线车道线的端点构建每个矩形车道单元；
8.步骤3：通过mems-ins的机械编排算法解算得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度，通过gnss伪距差分解算得到wgs-84坐标系下车辆近似位置；
9.步骤4：计算wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离，将最短距离对应的矩形车道单元作为车辆近似位置落在的矩形车道单元，将车辆近似位置落在的矩形车道单元记为起始搜索单元；
10.步骤5：以起始搜索单元为搜索起点并选定搜索模糊度，根据搜索模糊度划定的范围，在先验地图上选定多个矩形车道单元作为车辆真实位置可能位于的矩形车道单元作为候选单元，并记录所有候选单元对应的虚线车道线端点的三维坐标；
11.步骤6：通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，得到车辆距离最短的车道线的端点像素坐标，结合通过双目视觉立体校正方法进一步计算相机坐标系下车辆相对于车辆距离最短的虚线车道线端点的距离信息；
12.步骤7：构建每组矩形车道单元对应的gvim融合导航的量测方程，使用lambda方法分别求解每组量测方程得到当前时刻车辆的候选位置和姿态，选取最优解作为当前时刻车辆的位姿；
13.作为优选，步骤2所述每条虚线车道线的端点构建每个矩形车道单元，具体如下：
14.在高精度地图数据中标记每条虚线车道线的端点的三维坐标，每条虚线车道线的端点的三维坐标均位于wgs-84坐标系下；
15.遵循lanelets地图框架将车道编码为多个lanelet车道单元，每个lanelet车道单元由2个或4个每条虚线车道线的端点作为边界点定义；
16.作为优选，步骤3所述通过mems-ins的机械编排算法解算得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度，具体如下：
17.获取车辆实时角速率增量、车辆实时速度增量，将车辆实时角速率增量、车辆实时速度增量通过mems-ins的机械编排算法进行积分计算，得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度；
18.步骤3所述通过gnss伪距差分解算得到wgs-84坐标系下车辆近似位置，具体如下：
19.获取车载gnsss原始观测数据，通过gnss伪距差分解算方法定位，获得wgs-84坐标系下车辆近似位置；
20.作为优选，步骤4所述计算wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离，具体如下：
21.将wgs-84坐标系下车辆近似位置与每条虚线车道线的端点依次通过欧式距离计算，得到wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离；
22.步骤4所述将最短距离对应的矩形车道单元作为车辆近似位置落在的矩形车道单元，具体如下：
23.选取wgs-84坐标系下车辆初始位置到虚线车道线的端点的距离最短对应的虚线车道线的端点，并根据lanelets地图框架下拓扑映射关系得到距离最短对应的虚线车道线的端点所属的矩形车道单元，该单元即为车辆初始位置落在的矩形车道单元；
24.若距离最短对应的虚线车道线的端点位于多个矩形车道单元内，计算车辆航向角与各矩形车道单元方向之间夹角，矩形车道单元的方向由车辆左侧边界最接近的四个点进行最佳的直线拟合来计算；
25.最佳拟合的直线方向即为该矩形车道单元的方向，车辆航向角与各矩形车道单元方向之间最小夹角对应的矩形车道单元的即为近似位置最有可能落在的矩形车道单元；
26.作为优选，步骤5所述以起始搜索单元为搜索起点并选定搜索模糊度，具体如下：
27.在先验地图中，以起始搜索单元作为起点，横向上存在n
x
条平行车道；
28.纵向上，以起始搜索单元为中心，选取ny行车道单元；
29.所述模糊搜索度为：n
ll
＝n
x
·
ny；
30.步骤5所述根据搜索模糊度划定的范围，在先验地图上选定多个矩形车道单元作为车辆真实位置可能位于的矩形车道单元作为候选单元，并记录所有候选单元对应的虚线车道线端点的三维坐标，具体如下：
31.所述矩形车道单元的数量为n
ll
；
32.搜索模糊度n
ll
；
33.以起始搜索单元为搜索起点，在横向上具有n
x
个平行车道，纵向上具有ny行lanelet单元；
34.假设n
ll
个lanelet矩形单元对应的车道是载体真实位置可能存在的车道；
35.根据n
ll
个lanelet矩形单元，得到n
ll
组虚线车道线端点的三维坐标作为候选se，
记为：
[0036][0037]
其中，为wgs-84坐标系下第k个矩形车道单元对应的虚线车道线端点的三维坐标，分别为wgs-84坐标系下第k个矩形车道单元对应的虚线车道线端点的x轴坐标、y轴坐标、z轴坐标,k∈[1,n
ll
]；
[0038]
作为优选，步骤6所述通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，具体如下：
[0039]
获取车辆实时双目视觉图像，将车辆实时双目视觉图像通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，车辆距离最短的车道线的端点像素坐标；
[0040]
步骤6所述进一步计算相机坐标系下车辆相对于车辆距离最短的虚线车道线端点的距离信息，具体如下：
[0041]
将车辆实时双目视觉图像、车辆距离最短的虚线车道线端点像素坐标通过双目视觉立体校正方法，得到车辆距离最短的虚线车道线端点的深度信息，根据像素坐标和深度信息获得相机坐标系下车辆到距离车辆最近的虚线车道线端点的距离信息；
[0042]
作为优选，步骤7所述构建每组矩形车道单元对应的gvim融合导航的量测方程，具体如下:
[0043]
将n
ll
组虚线车道线端点的三维坐标转到以rtk参考站为原点的站心坐标系的g系下，记为：
[0044][0045]
其中，为g系下第k个矩形车道单元对应的虚线车道线端点的三维坐标，分别为g系下第k个矩形车道单元对应的虚线车道线端点的x轴坐标、y轴坐标、z轴坐标,k∈[1,n
ll
]；
[0046]
步骤6所述的车辆相对于车辆距离最短的虚线车道线端点的距离信息在导航系下定义为：(xn,yn,zn)
[0047]
其中，xn,yn,zn分别为车辆到距离车辆最近的虚线车道线端点的距离在导航系下的x轴投影、y轴投影、z轴投影；
[0048]
将(xn,yn,zn)、步骤3所述的wgs-84坐标系下车辆近似位置引入到gnss-rtk的双差分码伪距和载波相位测量模型中，进一步使用一阶泰勒级数展开，得到步骤7所述的每组矩形车道单元对应的gvim融合导航的量测方程，具体如下：
[0049][0050]
[0051][0052]
其中，i表示卫星i，j表示卫星j，v表示车辆，r表示rtk参考站，表示车辆与rtk参考站之间卫星i与卫星j对应的双差伪距测量值，分别为g系下的x轴、y轴、z轴上的一阶泰勒展开的线性化系数，为wgs-84坐标系到g系的旋转矩阵，分别为g系下的x轴向、y轴向的、z轴向的的基线分量，表示车辆与rtk参考站之间卫星i与卫星j对应的初始双差卫地距，λ是卫星信号波长，表示车辆与rtk参考站之间卫星i与卫星j对应的双差相位测量值，表示车辆与rtk参考站之间卫星i与卫星j对应的双差分载波相位模糊度，分别为g系下车辆近似位置的x轴坐标、y轴坐标、z轴坐标，为载体坐标系b系到导航系n系的旋转矩阵，γ0,θ0,分别表示步骤4得到的车辆的近似俯仰角，横滚角和航向角，表示车辆航向角参数，(l
x
,ly,lz)为导航系下相机到gnss/ins组合导航设备的杆臂，l
x
,ly,lz分别为导航系下相机到gnss/ins组合导航设备的右向杆臂、前向y轴杆臂、天向z轴杆臂；
[0053]
步骤7所述使用lambda方法分别求解每组量测方程得到当前时刻车辆的候选位置和姿态，具体如下：
[0054]
将每个候选单元对应的gvim融合导航的量测方程依次使用lambda方法求解，得到每个候选单元对应的位置解和姿态解，以及模糊度固定比率；
[0055]
在n
ll
组求解结果中，选取模糊度固定比率最大的求解结果作为最优解；
[0056]
将最优解中车辆的位置求解结果作为当前时刻车辆位置；
[0057]
将最优解中车辆的姿态求解结果作为当前时刻车辆姿态。
[0058]
本发明还提供了一种计算机可读介质，所述计算机可读介质存储电子设备执行的计算机程序，当所述计算机程序在电子设备上运行时，执行所述地图辅助的多传感器融合定位方法的步骤。
[0059]
本发明提出以下改进：
[0060]
针对当前高精度地图数据的简化与快速搜索问题，本文提出一种用于辅助定位的车道级高精度地图构建方法。使用lanelets格式的高精度地图框架，对高精度地图中的车道线端点进行重新编码并保存在xml文件中。地图加载通过xml文件引入到系统中。
[0061]
针对复杂城市道路环境下，gnss退化导致车辆定位不准乃至定位丢失的问题，本发明基于车道级先验地图实现了gnss-rtk/mems-ins/视觉的紧组合方法，提取了一种gnss/ins/视觉的紧耦合数学模型。传统的车道线辅助gnss/ins组合导航定位方法无法获得视觉识别的se的绝对位置，因此只能修正载体在车道内的横向误差。gvim紧组合融合定位方法通过基于地图的模糊搜索，假设载体可能所处的lanelet单元获得对应se的绝对位置，将视觉获得的相对距离信息引入gnss-rtk基线解算中，实现了对车道内载体的三维位置约束，进而提高了gnss退化场景中的定位精度。
[0062]
本发明方案实施简单方便，实用性强，解决了相关技术存在的实用性低及实际应用不便的问题，能够提高用户体验，具有重要的市场价值。
附图说明
[0063]
图1：本发明实施例的方法流程图。
具体实施方式
[0064]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0065]
具体实施时，本发明技术方案提出的方法可由本领域技术人员采用计算机软件技术实现自动运行流程，实现方法的系统装置例如存储本发明技术方案相应计算机程序的计算机可读存储介质以及包括运行相应计算机程序的计算机设备，也应当在本发明的保护范围内。
[0066]
下面结合图1介绍本发明实施例的技术方案为一种多基准站网的gnss基线联合估测方法，具体如下：
[0067]
参见图1，本发明实施例的方法流程图。
[0068]
步骤1：获取高精度地图数据；
[0069]
步骤2：将高精度地图数据在lanelets地图框架下，结合每条虚线车道线的端点构建每个矩形车道单元；
[0070]
步骤2所述每条虚线车道线的端点构建每个矩形车道单元，具体如下：
[0071]
在高精度地图数据中标记每条虚线车道线的端点的三维坐标，每条虚线车道线的端点的三维坐标均位于wgs-84坐标系下；
[0072]
遵循lanelets地图框架将车道编码为多个lanelet车道单元，每个lanelet车道单元由2个或4个每条虚线车道线的端点作为边界点定义；
[0073]
步骤3：通过mems-ins的机械编排算法解算得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度，通过gnss伪距差分解算得到wgs-84坐标系下车辆近似位置；
[0074]
步骤3所述通过mems-ins的机械编排算法解算得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度，具体如下：
[0075]
获取车辆实时角速率增量、车辆实时速度增量，将车辆实时角速率增量、车辆实时速度增量通过mems-ins的机械编排算法进行积分计算，得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度；
[0076]
步骤3所述通过gnss伪距差分解算得到wgs-84坐标系下车辆近似位置，具体如下：
[0077]
获取车载gnsss原始观测数据，通过gnss伪距差分解算方法定位，获得wgs-84坐标系下车辆近似位置；
[0078]
步骤4：计算wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离，将最短距离对应的矩形车道单元作为车辆近似位置落在的矩形车道单元，将车辆近似位置落在
的矩形车道单元记为起始搜索单元；
[0079]
步骤4所述计算wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离，具体如下：
[0080]
将wgs-84坐标系下车辆近似位置与每条虚线车道线的端点依次通过欧式距离计算，得到wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离；
[0081]
步骤4所述将最短距离对应的矩形车道单元作为车辆近似位置落在的矩形车道单元，具体如下：
[0082]
选取wgs-84坐标系下车辆初始位置到虚线车道线的端点的距离最短对应的虚线车道线的端点，并根据lanelets地图框架下拓扑映射关系得到距离最短对应的虚线车道线的端点所属的矩形车道单元，该单元即为车辆初始位置落在的矩形车道单元；
[0083]
若距离最短对应的虚线车道线的端点位于多个矩形车道单元内，计算车辆航向角与各矩形车道单元方向之间夹角，矩形车道单元的方向由车辆左侧边界最接近的四个点进行最佳的直线拟合来计算；
[0084]
最佳拟合的直线方向即为该矩形车道单元的方向，车辆航向角与各矩形车道单元方向之间最小夹角对应的矩形车道单元的即为近似位置最有可能落在的矩形车道单元；
[0085]
步骤5：以起始搜索单元为搜索起点并选定搜索模糊度，根据搜索模糊度划定的范围，在先验地图上选定多个矩形车道单元作为车辆真实位置可能位于的矩形车道单元作为候选单元，并记录所有候选单元对应的虚线车道线端点的三维坐标；
[0086]
步骤5所述以起始搜索单元为搜索起点并选定搜索模糊度，具体如下：
[0087]
在先验地图中，以起始搜索单元作为起点，横向上存在n
x
＝3条平行车道；
[0088]
纵向上，以起始搜索单元为中心，选取ny＝5行车道单元；
[0089]
所述模糊搜索度为：n
ll
＝n
x
·
ny；
[0090]
步骤5所述根据搜索模糊度划定的范围，在先验地图上选定多个矩形车道单元作为车辆真实位置可能位于的矩形车道单元作为候选单元，并记录所有候选单元对应的虚线车道线端点的三维坐标，具体如下：
[0091]
所述矩形车道单元的数量为n
ll
；
[0092]
搜索模糊度n
ll
；
[0093]
以起始搜索单元为搜索起点，在横向上具有n
x
个平行车道，纵向上具有ny行lanelet单元；
[0094]
假设n
ll
个lanelet矩形单元对应的车道是载体真实位置可能存在的车道；
[0095]
根据n
ll
个lanelet矩形单元，得到n
ll
组虚线车道线端点的三维坐标作为候选se，记为：
[0096][0097]
其中，为wgs-84坐标系下第k个矩形车道单元对应的虚线车道线端点的三维坐标，分别为wgs-84坐标系下第k个矩形车道单元对应的虚线车道线端点的x轴坐标、y轴坐标、z轴坐标,k∈[1,n
ll
]；
[0098]
步骤6：通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，得到车辆距离最短的车道线的端点像素坐标，结合通过双目视觉立体校正方法进一步计算相机坐标
系下车辆相对于车辆距离最短的虚线车道线端点的距离信息；
[0099]
步骤6所述通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，具体如下：
[0100]
获取车辆实时双目视觉图像，将车辆实时双目视觉图像通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，车辆距离最短的车道线的端点像素坐标；
[0101]
步骤6所述进一步计算相机坐标系下车辆相对于车辆距离最短的虚线车道线端点的距离信息，具体如下：
[0102]
将车辆实时双目视觉图像、车辆距离最短的虚线车道线端点像素坐标通过双目视觉立体校正方法，得到车辆距离最短的虚线车道线端点的深度信息，根据像素坐标和深度信息获得相机坐标系下车辆到距离车辆最近的虚线车道线端点的距离信息；
[0103]
步骤7：构建每组矩形车道单元对应的gvim融合导航的量测方程，使用lambda方法分别求解每组量测方程得到当前时刻车辆的候选位置和姿态，选取最优解作为当前时刻车辆的位姿；
[0104]
步骤7所述构建每组矩形车道单元对应的gvim融合导航的量测方程，具体如下:
[0105]
将n
ll
组虚线车道线端点的三维坐标转到以rtk参考站为原点的站心坐标系的g系下，记为：
[0106][0107]
其中，为g系下第k个矩形车道单元对应的虚线车道线端点的三维坐标，分别为g系下第k个矩形车道单元对应的虚线车道线端点的x轴坐标、y轴坐标、z轴坐标，k∈[1,n
ll
]；
[0108]
步骤6所述的车辆相对于车辆距离最短的虚线车道线端点的距离信息在导航系下定义为：(xn,yn,zn)
[0109]
其中，xn,yn,zn分别为车辆到距离车辆最近的虚线车道线端点的距离在导航系下的x轴投影、y轴投影、z轴投影；
[0110]
将(xn,yn,zn)、步骤3所述的wgs-84坐标系下车辆近似位置引入到gnss-rtk的双差分码伪距和载波相位测量模型中，进一步使用一阶泰勒级数展开，得到步骤7所述的每组矩形车道单元对应的gvim融合导航的量测方程，具体如下：
[0111][0112][0113]
[0114]
其中，i表示卫星i，j表示卫星j，v表示车辆，r表示rtk参考站，表示车辆与rtk参考站之间卫星i与卫星j对应的双差伪距测量值，分别为g系下的x轴、y轴、z轴上的一阶泰勒展开的线性化系数，为wgs-84坐标系到g系的旋转矩阵，分别为g系下的x轴向、y轴向的、z轴向的的基线分量，表示车辆与rtk参考站之间卫星i与卫星j对应的初始双差卫地距，λ是卫星信号波长，表示车辆与rtk参考站之间卫星i与卫星j对应的双差相位测量值，表示车辆与rtk参考站之间卫星i与卫星j对应的双差分载波相位模糊度，分别为g系下车辆近似位置的x轴坐标、y轴坐标、z轴坐标，为载体坐标系b系到导航系n系的旋转矩阵，γ0,θ0,分别表示步骤4得到的车辆的近似俯仰角，横滚角和航向角，表示车辆航向角参数，(l
x
,ly,lz)为导航系下相机到gnss/ins组合导航设备的杆臂，l
x
,ly,lz分别为导航系下相机到gnss/ins组合导航设备的右向杆臂、前向y轴杆臂、天向z轴杆臂；
[0115]
步骤7所述使用lambda方法分别求解每组量测方程得到当前时刻车辆的候选位置和姿态，具体如下：
[0116]
将每个候选单元对应的gvim融合导航的量测方程依次使用lambda方法求解，得到每个候选单元对应的位置解和姿态解，以及模糊度固定比率；
[0117]
在n
ll
组求解结果中，选取模糊度固定比率最大的求解结果作为最优解；
[0118]
将最优解中车辆的位置求解结果作为当前时刻车辆位置；
[0119]
将最优解中车辆的姿态求解结果作为当前时刻车辆姿态。
[0120]
本发明的具体实施例还提供了一种计算机可读介质。
[0121]
所述计算机可读介质为服务器工作站；
[0122]
所述服务器工作站存储电子设备执行的计算机程序，当所述计算机程序在电子设备上运行时，使得所述电子设备执行本发明实施例的地图辅助的多传感器融合定位方法的步骤。
[0123]
应当理解的是，本说明书未详细阐述的部分均属于现有技术。
[0124]
应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

技术特征：
1.一种地图辅助的多传感器融合定位方法，其特征在于：将高精度地图数据在lanelets地图框架下，结合每条虚线车道线的端点构建每个矩形车道单元；确定车辆近似位置落在的矩形车道单元，并记为起始搜索单元；以起始搜索单元为搜索起点，根据搜索模糊度划定的范围搜索得到多个候选单元；构建每组矩形车道单元对应的gvim融合导航的量测方程，使用lambda方法优化求解得到当前时刻车辆的位姿。2.根据权利要求1所述的地图辅助的多传感器融合定位方法，其特征在于，包括以下步骤：步骤1：获取高精度地图数据；步骤2：将高精度地图数据在lanelets地图框架下，结合每条虚线车道线的端点构建每个矩形车道单元；步骤3：通过mems-ins的机械编排算法解算得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度，通过gnss伪距差分解算得到wgs-84坐标系下车辆近似位置；步骤4：计算wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离，将最短距离对应的矩形车道单元作为车辆近似位置落在的矩形车道单元，将车辆近似位置落在的矩形车道单元记为起始搜索单元；步骤5：以起始搜索单元为搜索起点并选定搜索模糊度，根据搜索模糊度划定的范围，在先验地图上选定多个矩形车道单元作为车辆真实位置可能位于的矩形车道单元作为候选单元，并记录所有候选单元对应的虚线车道线端点的三维坐标；步骤6：通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，得到车辆距离最短的车道线的端点像素坐标，结合通过双目视觉立体校正方法进一步计算相机坐标系下车辆相对于车辆距离最短的虚线车道线端点的距离信息；步骤7：构建每组矩形车道单元对应的gvim融合导航的量测方程，使用lambda方法分别求解每组量测方程得到当前时刻车辆的候选位置和姿态，选取最优解作为当前时刻车辆的位姿。3.根据权利要求2所述的地图辅助的多传感器融合定位方法，其特征在于：步骤2所述每条虚线车道线的端点构建每个矩形车道单元，具体如下：在高精度地图数据中标记每条虚线车道线的端点的三维坐标，每条虚线车道线的端点的三维坐标均位于wgs-84坐标系下；遵循lanelets地图框架将车道编码为多个lanelet车道单元，每个lanelet车道单元由2个或4个每条虚线车道线的端点作为边界点定义。4.根据权利要求3所述的地图辅助的多传感器融合定位方法，其特征在于：步骤3所述通过mems-ins的机械编排算法解算得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度，具体如下：获取车辆实时角速率增量、车辆实时速度增量，将车辆实时角速率增量、车辆实时速度增量通过mems-ins的机械编排算法进行积分计算，得到载体导航系下的车辆近似实时姿态、载体导航系下的车辆近似实时速度；
步骤3所述通过gnss伪距差分解算得到wgs-84坐标系下车辆近似位置，具体如下：获取车载gnsss原始观测数据，通过gnss伪距差分解算方法定位，获得wgs-84坐标系下车辆近似位置。5.根据权利要求4所述的地图辅助的多传感器融合定位方法，其特征在于：步骤4所述计算wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离，具体如下：将wgs-84坐标系下车辆近似位置与每条虚线车道线的端点依次通过欧式距离计算，得到wgs-84坐标系下车辆近似位置到每条虚线车道线的端点的距离；步骤4所述将最短距离对应的矩形车道单元作为车辆近似位置落在的矩形车道单元，具体如下：选取wgs-84坐标系下车辆初始位置到虚线车道线的端点的距离最短对应的虚线车道线的端点，并根据lanelets地图框架下拓扑映射关系得到距离最短对应的虚线车道线的端点所属的矩形车道单元，该单元即为车辆初始位置落在的矩形车道单元；若距离最短对应的虚线车道线的端点位于多个矩形车道单元内，计算车辆航向角与各矩形车道单元方向之间夹角，矩形车道单元的方向由车辆左侧边界最接近的四个点进行最佳的直线拟合来计算；最佳拟合的直线方向即为该矩形车道单元的方向，车辆航向角与各矩形车道单元方向之间最小夹角对应的矩形车道单元的即为近似位置最有可能落在的矩形车道单元。6.根据权利要求5所述的地图辅助的多传感器融合定位方法，其特征在于：步骤5所述以起始搜索单元为搜索起点并选定搜索模糊度，具体如下：在先验地图中，以起始搜索单元作为起点，横向上存在n
x
条平行车道；纵向上，以起始搜索单元为中心，选取n
y
行车道单元；所述模糊搜索度为：n
ll
＝
x
·
y
；步骤5所述根据搜索模糊度划定的范围，在先验地图上选定多个矩形车道单元作为车辆真实位置可能位于的矩形车道单元作为候选单元，并记录所有候选单元对应的虚线车道线端点的三维坐标，具体如下：所述矩形车道单元的数量为n
ll
；搜索模糊度n
ll
；以起始搜索单元为搜索起点，在横向上具有n
x
个平行车道，纵向上具有n
y
行lanelet单元；假设n
ll
个lanelet矩形单元对应的车道是载体真实位置可能存在的车道；根据n
ll
个lanelet矩形单元，得到n
ll
组虚线车道线端点的三维坐标作为候选se，记为：其中，为wgs-84坐标系下第k个矩形车道单元对应的虚线车道线端点的三维坐标，分别为wgs-84坐标系下第k个矩形车道单元对应的虚线车道线端点的x轴坐标、y轴坐标、z轴坐标,k∈[1,n
ll
]。7.根据权利要求6所述的地图辅助的多传感器融合定位方法，其特征在于：步骤6所述通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，具体如下：
获取车辆实时双目视觉图像，将车辆实时双目视觉图像通过具有车道宽度约束的快速视觉自我车道检测方法进行提取，车辆距离最短的车道线的端点像素坐标；步骤6所述进一步计算相机坐标系下车辆相对于车辆距离最短的虚线车道线端点的距离信息，具体如下：将车辆实时双目视觉图像、车辆距离最短的虚线车道线端点像素坐标通过双目视觉立体校正方法，得到车辆距离最短的虚线车道线端点的深度信息，根据像素坐标和深度信息获得相机坐标系下车辆到距离车辆最近的虚线车道线端点的距离信息。8.根据权利要求7所述的地图辅助的多传感器融合定位方法，其特征在于：步骤7所述构建每组矩形车道单元对应的gvim融合导航的量测方程，具体如下:将n
ll
组虚线车道线端点的三维坐标转到以rtk参考站为原点的站心坐标系的g系下，记为：其中，为g系下第k个矩形车道单元对应的虚线车道线端点的三维坐标，分别为g系下第k个矩形车道单元对应的虚线车道线端点的x轴坐标、y轴坐标、z轴坐标,k∈[1,n
ll
]；步骤6所述的车辆相对于车辆距离最短的虚线车道线端点的距离信息在导航系下定义为：(x
n
,y
n
,z
n
)其中，x
n
,y
n
,z
n
分别为车辆到距离车辆最近的虚线车道线端点的距离在导航系下的x轴投影、y轴投影、z轴投影；将(x
n
,y
n
,z
n
)、步骤3所述的wgs-84坐标系下车辆近似位置引入到gnss-rtk的双差分码伪距和载波相位测量模型中，进一步使用一阶泰勒级数展开，得到步骤7所述的每组矩形车道单元对应的gvim融合导航的量测方程，具体如下：开，得到步骤7所述的每组矩形车道单元对应的gvim融合导航的量测方程，具体如下：开，得到步骤7所述的每组矩形车道单元对应的gvim融合导航的量测方程，具体如下：其中，i表示卫星i，j表示卫星j，v表示车辆，r表示rtk参考站，表示车辆与rtk参考站之间卫星i与卫星j对应的双差伪距测量值，分别为g系下的x轴、y轴、z轴上的一阶泰勒展开的线性化系数，为wgs-84坐标系到g系的旋转矩阵，分别为g系下的x轴向、y轴向的、z轴向的的基线分量，表示车辆与rtk参考站之间卫星i与卫
星j对应的初始双差卫地距，λ是卫星信号波长，表示车辆与rtk参考站之间卫星i与卫星j对应的双差相位测量值，表示车辆与rtk参考站之间卫星i与卫星j对应的双差分载波相位模糊度，分别为g系下车辆近似位置的x轴坐标、y轴坐标、z轴坐标，为载体坐标系b系到导航系n系的旋转矩阵，γ0,θ0,分别表示步骤4得到的车辆的近似俯仰角，横滚角和航向角，表示车辆航向角参数，(l
x
,l
y
,l
z
)为导航系下相机到gnss/ins组合导航设备的杆臂，l
x
,l
y
,l
z
分别为导航系下相机到gnss/ins组合导航设备的右向杆臂、前向y轴杆臂、天向z轴杆臂。9.根据权利要求8所述的地图辅助的多传感器融合定位方法，其特征在于：步骤7所述使用lambda方法分别求解每组量测方程得到当前时刻车辆的候选位置和姿态，具体如下：将每个候选单元对应的gvim融合导航的量测方程依次使用lambda方法求解，得到每个候选单元对应的位置解和姿态解，以及模糊度固定比率；在n
ll
组求解结果中，选取模糊度固定比率最大的求解结果作为最优解；将最优解中车辆的位置求解结果作为当前时刻车辆位置；将最优解中车辆的姿态求解结果作为当前时刻车辆姿态。10.一种计算机可读介质，其特征在于，其存储电子设备执行的计算机程序，当所述计算机程序在电子设备上运行时，使得所述电子设备执行如权利要求1-9任一项所述方法的步骤。

技术总结
本发明提出了一种地图辅助的多传感器融合定位方法及计算机可读介质。本发明获取高精度地图数据；将高精度地图数据在Lanelets地图框架下，结合每条虚线车道线的端点构建每个矩形车道单元；通过解算得到WGS-84坐标系下车辆近似位置；将车辆近似位置落在的矩形车道单元记为起始搜索单元；结合起始搜索单元计算所有候选单元对应的虚线车道线端点的三维坐标；计算相机坐标系下车辆相对于车辆距离最短的虚线车道线端点的距离信息；构建每组矩形车道单元对应的GVIM融合导航的量测方程，使用LAMBDA方法分别优化求解量测方程得到当前时刻车辆的位姿。本发明提高了GNSS退化场景下的定位精度，实现连续可靠、低成本的车道级导航定位，服务于车道级别的智能载体规划控制。务于车道级别的智能载体规划控制。务于车道级别的智能载体规划控制。


技术研发人员：张红娟 钱闯 李必军 张兴 宋贞贞
受保护的技术使用者：武汉大学
技术研发日：2023.06.14
技术公布日：2023/10/11