三维激光雷达辅助的高精度卫星定位方法

1.本公开主要涉及用于智能交通系统的自动驾驶或其他种类的自主系统的领域。具体而言，本公开涉及一种用于nlos检测和校正的3d lidar辅助的全球导航卫星系统及方法，其可以提高定位性能。


背景技术：

2.自动驾驶被公认为是过度交通拥堵和预期事故的补救措施。然而，当前解决方案的定位精度不足是阻碍自动驾驶在城市场景中到来的关键问题之一。随着对adv的需求不断增加，在城市环境中的定位变得至关重要。
3.gnss，例如gps和/或其他类似的基于卫星的定位技术，目前是为智能交通系统的adv定位提供全球参考定位的主要手段之一。随着多个卫星星座的可用性增加，gnss可以在开阔的天空区域提供令人满意的性能。然而，当大部分天空被遮挡时，gnss的性能会严重下降，这是一个具有挑战性的问题。这种情况被称为“城市峡谷”情景。通常，在高度城市化的城市中，由于静态建筑物和动态对象引起的信号反射，定位精度会显著下降。
4.特别地，gnss-rtk用于4级全自动驾驶车辆的高精度航空测绘和定位。通常，gnss-rtk定位涉及两个步骤：(1)根据接收到的gnss测量值估计浮点解；(2)基于导出的浮点解作为初始猜测，使用最小二乘算法(例如lambda)求解整周模糊度。在实现固定解的情况下，基于空旷区域的双差分载波和编码测量，可以达到厘米级的定位精度。不幸的是，由于gnss信号反射和周围建筑物的遮挡导致的nlos和多径接收，gnss-rtk的准确性在城市峡谷中显著降低。在实践中，城市峡谷中gnss-rtk定位精度的显著下降主要是由gnss nlos接收的发生导致的。由于nlos接收，gnss定位的性能受到建筑物和动态对象等实时周围环境特征的高度影响。部分接收到的gnss信号受到严重污染，包括大噪声。根据发明人之前的研究，在高度城市化的地区，接收到的大部分gnss信号可以是多径或nlos接收。因此，基于差分载波和编码测量的浮点解估计的准确性降低，使得模糊度求解难以得到固定解。
5.此外，由于来自周围建筑物的信号遮挡，城市峡谷中可用卫星的数量受到限制。因此，卫星分布的几何形状被扭曲，导致大的dop。结果，由于卫星几何形状较差，搜索空间模糊度较大，难以获得固定解。简而言之，城市峡谷场景给gnss-rtk定位的两个步骤都带来了额外的困难。
6.因此，有效地感测和理解周围环境是改善城市地区的gnss定位的关键，因为gnss定位严重依赖于天空视野可见性。应对gnss nlos接收最著名的方法是3dmagnss定位，例如基于3d地图构建信息的nlos排除和阴影匹配。然而，这些3dma gnss方法的缺点是1)依赖3d建筑模型的可用性和gnss接收器位置的初始猜测；2)无法缓解由周围动态对象引起的nlos接收。3dma gnss定位方法的最新进展在发明人之前的工作中进行了详细回顾。
7.在发明人最近的出版物中，3d lidar传感器，即被称之为adv的“眼睛”，自动驾驶车辆的典型不可或缺的车载传感器，已被用于检测由动态对象引起的nlos。由于3d lidar的-30
°
～+10
°
fov有限，只能扫描双层巴士的一部分。此外，该方法严重依赖对象检测的准
确性。尽管如此，这是第一项采用实时对象检测来辅助gnss定位的工作。探索了使用实时3d lidar点云检测周围建筑物的方法，而不是仅检测动态对象。由于3d lidar的视野有限，只能扫描部分建筑物。因此，需要有关建筑物高度的信息来检测由建筑物引起的nlos接收。发明人没有排除检测到的nlos接收，而是探索了在lidar的帮助下校正nlos伪距测量值的替代方案。3d lidar可以测量从gnss接收器到可能已反射gnss信号的建筑物表面的距离。然后，校正后的和剩余的健康gnss测量值都可以用于进一步的gnss定位。在校正检测到的nlos卫星后获得了改进的性能。然而，这种方法的性能依赖于建筑物和反射器检测的准确性。当建筑物表面高度不规则时，建筑物检测和反射器检测都可能失败。lidar有限的fov在动态对象和建筑物的检测中仍然是一个缺点。总体而言，以前的工作显示了使用实时机载传感(实时点云)检测gnss nlos的可行性。为了克服3d lidar的有限fov的缺点，发明人探索了使用鱼眼相机和3d lidar来检测和校正nlos信号。鱼眼相机用于检测非视距信号。同时，3d lidar被用来测量gnss接收器与导致nlos信号的潜在反射器之间的距离。然而，这种方法存在nlos检测对环境光照条件敏感的问题。
8.因此，本领域需要具有改进的定位方法和系统，特别是对于能够在非常深的城市峡谷中实现高精度定位的自动驾驶。此外，结合本公开的附图和背景，从随后的详细描述和所附权利要求中，其他期望的特征和特性将变得显而易见。


技术实现要素：

9.本文提供了一种3d lidar辅助gnss nlos缓解方法和实现该方法的系统。本公开的目的是提供一种缓解由静态建筑物和动态对象引起的nlos的方法。
10.本公开的第一方面提供了一种用于使用卫星定位系统支持车辆进行定位的方法。该方法包括：从3d lidar传感器和lio接收lidar因子和imu因子；使用局部因子图优化集成lidar因子和imu因子，以估计两个历元之间的相对运动；生成3d pcm作为辅助地标卫星，用于提供低仰角辅助地标卫星；通过gnss接收器从卫星接收gnss测量值；使用3d pcm从所述gnss测量值中检测gnss nlos接收；以及从所述gnss测量值中排除所述gnss nlos接收，以获得幸存的gnss卫星测量值，从而提高用于自动驾驶车辆定位的gnss测量值的质量。
11.在一实施例中，该方法进一步包括：对所述幸存的gnss卫星测量值执行gnss-rtk浮点估计，以获得浮点解；执行模糊度求解以获得固定模糊度解；以及从所述浮点解和所述固定模糊度解确定固定的gnss-rtk定位解。
12.在一实施例中，通过应用lambda算法来执行所述模糊度求解。
13.在一实施例中，该方法进一步包括：将所述固定的gnss-rtk定位解反馈给所述3d lidar传感器和所述lio；以及使用所述固定的gnss-rtk定位解执行pcm校正，以校正3d点云的漂移。
14.在一实施例中，该方法进一步包括：在所述gnss测量值上使用最小二乘算法获得所述浮点解的初始猜测。
15.本公开的第二方面提供了一种用于使用卫星定位系统支持车辆进行定位的方法。该方法包括：基于来自3d lidar传感器和ahrs的3d点云实时地生成滑动窗口地图(swm)，其中，所述swm提供用于检测和校正nlos接收的环境描述；将来自先前帧的3d点云累积到所述swm中，以增强所述3d lidar传感器的fov；通过gnss接收器接收来自卫星的gnss测量值；使
用所述swm从所述gnss测量值中检测所述nlos接收；当在所述swm中找不到反射点时，通过nlos重构来校正所述nlos接收；以及通过最小二乘算法估计gnss定位。
16.在一实施例中，该方法进一步包括：通过排除远离所述gnss接收器的点云来使所述swm的点云容量最小化，使得所述3d点云位于滑动窗口内。
17.在一实施例中，生成所述swm的步骤包括：基于来自3d lidar传感器的所述3d点云，从lidar扫描匹配中获取局部地图；以及采用ahrs的方向将所述swm从载体坐标系转换为局部东北天(enu)坐标系。
18.在一实施例中，使用快速搜索方法执行所述从所述gnss测量值中检测所述nlos接收的步骤，其中，所述快速搜索方法包括：使所述3d lidar传感器的中心的搜索点初始化；根据卫星的仰角和方位角确定连接所述gnss接收器和卫星的搜索方向；以固定的增量值δd
pix
沿所述搜索方向移动所述搜索点；计算所述搜索点附近的相邻点(nk)数量；以及如果nk超过预定阈值n
thres
，则将所述搜索点归类为nlos卫星。
19.在一实施例中，该方法进一步包括：通过使用基于所述swm的模型校准重新估计所述gnss测量值来校正所述nlos接收，其中，所述swm提供密集、离散和无组织的3d点云，而没有连续的建筑物表面或边界。
20.在一实施例中，模型校准包括：使用基于反射器检测算法的高效kdtree结构检测所述nlos接收对应的反射点，其中，所述反射器检测算法包括：遍历从0
°
到360
°
的所有方位角，方位角分辨率为α
res
，仰角为当连接点pj和卫星的视距没有被阻挡时，检测潜在反射器；以及检测所述gnss接收器和所述潜在反射器之间距离最短的唯一反射器。
21.在一实施例中，nlos使用具有比例因子的加权方案来执行所述nlos重构。所述比例因子用于对所述nlos接收进行去加权，所述加权方案包括以下定义：如果卫星被归类为los测量值，则根据卫星信噪比(snr)和仰角计算所述比例因子；如果所述卫星被归类为nlos测量值并且校正了伪距误差，则根据所述卫星snr和所述仰角计算所述比例因子；以及如果所述卫星被归类为nlos测量值但未检测到反射点，则根据所述卫星snr、所述仰角和比例因子kw计算所述比例因子。
22.提供本发明内容是为了以简化形式介绍概念的选择，这些概念将在下面的详细描述中做了进一步描述。本发明内容并非旨在识别所要求保护的主题的关键特征或基本特征，也不旨在用作确定所要求保护的主题的范围的辅助。本发明的其他方面和优点如以下实施例所示公开。
附图说明
23.附图包含用于进一步说明和阐明本公开的上述和其他方面、优点和特征的附图。应当理解，这些附图仅描绘了本公开的某些实施例并且不旨在限制其范围。还应当理解，这些附图是为了简单和清楚而示出的，并且不一定按比例描绘。现在将通过使用附图以附加的具体性和细节来描述和解释本公开，其中：
24.图1是根据本公开示例性实施例的3d lidar辅助gnss实时动态差分定位方法的概略图；
25.图2是根据本公开示例性实施例的3d lidar辅助gnss nlos缓解方法的概略图；
26.图3是根据本公开示例性实施例的生成的滑动窗口地图和实时3d点云的示例性演
示；
27.图4是本发明采用的坐标系的概略图。
28.图5是示出根据本公开的示例性实施例的基于生成的滑动窗口地图的nlos检测的概念图；
29.图6是通过感知世界对被污染的gnss信号进行重构和校正；
30.图7为本发明实施例的3d lidar辅助gnss实时动态差分定位方法应用前后的定位表现。
31.图8是根据本公开的实施例的被配置为利用3d lidar辅助的gnss nlos缓解方法并结合到车辆中的3d lidar辅助gnss的系统图。
具体实施方式
32.本公开主要向自动驾驶或具有使用卫星定位系统的导航要求的其他类型的自主系统的领域提供。车辆可以是adv，或配备adas的车辆。更具体地，但不限于，本公开提供了一种3d lidar辅助的gnss nlos缓解方法，该方法可以在非常深的城市峡谷中检测和校正nlos信号，并且该系统实现了该方法。本公开的目的是提供一种缓解由静态建筑物和动态对象引起的nlos的方法，从而在高度密集的城市化区域中实现高精度。
33.效果、优势、问题的解决方案以及可能导致任何效果、优势或解决方案出现或变得更加明显的任何要素不应被解释为任何或所有权利要求的关键的、必需的或基本的特征或要素。本发明仅由所附权利要求限定，包括在本技术未决期间作出的任何修改以及所发布的那些权利要求的所有等同物。
34.在随后的权利要求和本发明的前面的描述中，除非上下文由于表达语言或必要的暗示而另有要求，否则“包括”(“comprise”或其变体，例如“comprises”或“comprising”)以包容性方式使用，即，指定所述特征的存在，但不排除本发明的各种实施例中进一步特征的存在或添加。
35.如本文所用，术语“全球导航卫星系统”或“gnss”是指通用卫星辅助导航系统，其中，电子接收器可以使用由卫星发射的视距无线电信号以指定的精度确定它们的位置。gnss接收器可以接收和处理来自绕地球运行的多颗卫星的信号，以确定gnss接收器的位置，并位置转换确定车辆的位置。为了本发明的目的，除非另有说明，否则gnss接收器的状态和卫星的位置均以ecef坐标系表示。
36.术语“lidar传感器”是指能够通过将激光束发射到物体并测量激光束的反射部分以计算距离来测量从车辆到物体的距离的传感器。
37.术语“姿态航向参考系统”或“ahrs”是指具有一个或多个传感器的系统，该一个或多个传感器被配置为使用振动来测量基于垂直参考的车辆的方向、方位和/或加速度的变化。
38.本公开的第一实施例是检测和排除gnss nlos接收以进一步改进gnss-rtk定位。同时，采用改进后的gnss-rtk定位来修正3d点云的漂移，从而提高整体定位精度也具有重要意义。
39.图1提供了本公开中提出的3d lidar辅助的gnss-rtk定位方法的概略图。该系统包括两部分：(1)基于来自3d lidar和imu的云的实时环境描述生成，以及来自gnss-rtk解
s100a的校正；(2)基于实时环境描述的gnss nlos检测和排除，以及基于s100b幸存卫星的gnss-rtk定位。
40.在某些实施例中，使用3d lidar传感器110通过从本质上解决传统gnss-rtk由于信号反射和遮挡引起的问题，改进了城市峡谷中的gnss-rtk定位。首先，执行3d lidar传感器110和lio120，其接收并使用局部因子图优化141松散地集成lidar因子和imu因子，以估计两个历元之间的相对运动并生成3d pcm以提供环境描述。lidar因子从lidar扫描匹配111获得，imu因子从预积分121获得。这里使用的环境描述是本地环境描述。在某些实施例中，3d lidar传感器110是配置为以10hz的频率收集原始3d点云数据的velodyne 32，并且lio 120是配置为以100hz的频率收集数据的xsens ti-10 imu。并且，同时估计周围点云的位置。因此，通过使用3d pcm上的周围点云的位置估计来执行pcm校正142。效果是该方法可以有利地生成局部准确的pcm。
41.其次，在环境描述的帮助下检测和排除潜在的gnss nlos卫星。gnss接收器130接收来自卫星的gnss测量值，其可以使用传统的最小二乘算法131来导出浮点解的初始猜测。在某些实施例中，gnss接收器130是商业级u-blox f9p gnss接收器，用于以1hz的数据频率收集原始gps/北斗测量值。此外，novatel span-cpt、gnss(gps、glonass和北斗)rtk/ins(光纤陀螺仪，fog)综合导航系统可用于提供地面实况定位。使用ros收集和同步所有数据。
42.对于后续的gnss nlos检测，本公开的第一实施例不依赖于gnss接收器130的浮点解的初始猜测。相反，使用通过lidar或惯性积分与相关算法生成的3d pcm从gnss测量值中检测gnss nlos接收。因此，使用gnss nlos排除151可以缓解gnss测量值的质量差的问题，它基本上排除了潜在的gnss nlos接收以获得幸存的gnss卫星测量。
43.为了解决这个问题，本公开提出采用来自生成的点云地图的地标作为“辅助地标卫星”以从根本上改善卫星几何形状。有利地，辅助地标卫星和接收卫星作为点云地图互补，可以提供低仰角辅助地标卫星。这种低仰角辅助地标卫星通常对于物理接收的卫星是不可用的。为此，排除了受污染的gnss nlos卫星，并在辅助地标卫星的帮助下改进了卫星几何形状。
44.接下来，可以通过执行gnss-rtk浮点估计152以获得浮点解，然后执行模糊度求解153以获得固定模糊度解，基于幸存的gnss卫星测量来估计浮点解。在某些实施例中，模糊度求解153通过应用lambda算法来执行。最后，估计的固定的gnss-rtk定位解154被反馈到3d lidar传感器和lio以执行pcm校正142以进一步校正3d点云的漂移。通过结合由gnss nlos检测辅助的、改善的gnss-rtk定位，该方法可以从本质上纠正lidar或惯性积分生成的3d点云地图的漂移。因此，所提出的方法有效地结合了lio(以在短时间内局部准确的方式，并为gnss nlos检测提供环境描述)和gnss-rtk(无漂移，具有全球参考定位但受gnss nlos影响)的互补性。
45.本公开的第一实施例通过利用3d lidar传感器的感知能力来检测和排除潜在的gnss nlos接收来解决这个问题。这种方法即使在城市峡谷场景中也可以达到10厘米的精度，以满足自动驾驶的导航要求。在本公开的第二实施例中，提供了一种替代方法和系统，其采用3d lidar来帮助gnss单点定位。定位只能在5米范围内准确，不如第一实施例的gnss-rtk准确。
46.参考图2，图解说明了3d lidar辅助gnss nlos缓解方法的概略图。该系统包括两
部分：(1)基于来自3d lidar传感器和ahrs的3d点云的实时swm生成s200a；(2)基于实时环境描述的gnss nlos检测与校正s200b。
47.有利地，swm是基于来自3d lidar传感器110的实时3d点云生成的，其中，仅使用滑动窗口内的3d点云来生成swm，因为远离gnss接收器130的点云对于nlos检测是不必要的。因此，可以最小化swm的尺寸。首先使用3d lidar传感器110和ahrs160生成swm，其中，swm本质上提供了用于检测和校正nlos接收的环境描述。基于来自3d lidar传感器110的3d点云，从lidar扫描匹配211获得局部地图。直接采用来自ahrs 160的方向以将swm从载体坐标系转换为局部enu坐标系。
48.通过执行局部转换241，可以生成swm，该swm被实时执行以用于gnss nlos检测。这对于实现更好的环境描述能力至关重要，从而可以增强lidar传感的fov。在传统方法中，仅应用实时3d点云来进一步检测nls卫星。然而，nlos检测的能力受限于3d lidar传感器110的fov。在这方面，本公开的第二实施例通过将实时点云累积到swm中来改进现有技术的不足。可以有效增强3d lidar传感器110的fov。
49.参考如图3所示的感知数字世界，其显示了实时3d lidar点云和swm之间的差异。感知环境开启了一个新窗口，以揭示由周围高层建筑等造成的gnss信号传输的遮挡或反射的影响。3d lidar传感器110的fov有限，单帧3d点云只能有效检测建筑物的低洼部分和车辆(例如双层巴士)。简单地基于实时3d点云无法有效地对高仰角卫星的可见性进行分类。由于3d lidar传感器110的物理扫描角度分布，实时3d点云也是稀疏的。
50.利用本公开的swm，可以有效地改善上述问题。swm的地图点如图3所示(从swm中去除地面点以进行有效的nlos检测)。在swm的帮助下，截止高度角可以达到76
°
，因此可以对仰角小于76
°
的卫星的能见度进行分类。swm中的点云比原始实时3d点云要密集得多，这可以显著提高nlos检测的准确性。完整的swm地图的快照显示在图3的右上角。建筑物和动态对象，如双层巴士，甚至树木都包含在swm中，这些并未包含在3d建筑模型中。
51.为了生成基于实时3d点云的点云地图，通常使用slam方法。事实上，可以在短时间内以低漂移获得满意的精度。但是，误差会随着时间的推移而累积，在长期行进后会导致较大的误差，并且通常无法进行闭环。因此，在实际应用中，只有半径为250米左右的圆圈内的物体才能引起gnss nlos接收，而忽略远处的建筑物。
52.参照图4，示出了坐标系的变换。ecef坐标系固定在地球的中心。选择第一个点作为enu坐标系的参考，而lidar、ahrs和gnss接收器之间的外部参数是预先固定和校准的。l坐标系(enu)通过转换为g坐标系(ecef)。
53.本公开仅使用3d点云的最后nsw坐标系来生成滑动窗口地图。结果，地图生成的漂移误差因此被限制在一个很小的值上，并且由窗口的大小(nsw)决定。为了使具有大量动态对象的城市峡谷中的垂直方向上的明显漂移最小化，本公开采用绝对地面来约束垂直漂移。下面演示了swm生成算法的细节：
54.输入：从历元t-n
sw
+1到历元t的的一系列点云为3d lidar、ahrs和gnss接收器之间的外部参数。
55.输出：
56.步骤1：初始化
57.步骤2:swm生成:
58.首先，将所有点云准到以为第一坐标系的局部地图中；其次，利用以下等式，将局部地图内的一个点转换到接收器载体坐标系中的
[0059][0060]
第三，利用以下等式将局部地图内的一个点转换为enu坐标系中的转换为enu坐标系中的
[0061]
现在返回参考图2，对gnss伪距测量值进行gnss nlos检测和校正s200b的第二部分设置有四个阶段。首先，gnss nlos检测251用于基于使用swm的卫星可见性分类的模型验证，这可以有效地识别los卫星和nlos卫星。接下来，如果一颗卫星被归类为nlos，则执行作为模型校准阶段的gnssnlos校正252，其通过校正nlos伪距测量值(cnlos)来重新估计gnss测量值。但是，如果一颗卫星被归类为nlos，而其反射点不在swm(fnlos)内，则意味着nlos校正不可用。第三阶段是执行nlos重构253，这是一个模型修复阶段，通过对nlos测量值进行去加权以用于进一步定位。gnss定位通过最小二乘算法254基于伪距测量值来估计。
[0062]
通过模型验证251进行的gnss nlos检测
[0063]
不需要对象检测算法来恢复检测到的动态对象或建筑物表面的实际高度。由于swm仅提供无组织的离散点，因此基于实时swm使用快速搜索的方法直接检测nlos接收，不需要对象检测过程。
[0064]
快速搜索方法的输入包括：卫星s的仰角历元t处的方位角最大搜索距离d
thres
和恒定增量值δd
pix
。快速搜索方法用于确定卫星s的卫星可见度在步骤1中，在enu坐标系中由示的3d lidar中心，初始化搜索点。连接gnss接收器130和卫星的搜索方向由卫星的仰角和方位角确定。swm被转换为kdtree结构用于查找相邻点。kdtree是一种用于点云处理的特殊结构，可以在搜索相邻点时高效执行。在步骤2中，给定一个固定的增量值δd
pix
，利用以下等式，根据图5示例性所示的搜索方向将搜索点移动到下一个点
[0065][0066][0067][0068]
k表示搜索点的索引。计算搜索点附近的相邻点(nk)的数量。根据步骤3，如果nk超过预定阈值n
thres
,，则在搜索点附近有一些来自建筑物或动态对象的地图点，并且连接gnss接收器130和卫星的视距被认为是被阻挡的。因此，卫星s被归类为nlos卫星。否则，重复上述步骤2和3。如果kδd
pix
》d
thres
，则gnss接收器130和卫星之间的方向是沿着los。d
thres
可以设置为定义距离，其中，在该距离内的位置被考虑用于nlos检测。使用快速
搜索方法，可以对卫星能见度进行分类。
[0069]
具有nlos和los卫星的卫星可见性分类结果的演示示于图6中。灰色圆圈代表受污染(多径效应)的gnss卫星，白色圆圈代表健康的gnss卫星。每个圆圈旁边提供的数字表示每颗卫星的仰角。从这个例子中，检测到54
°
的仰角。如前所述，最大截止高度角为76
°
，而基于swm的仰角可能与街道宽度显著相关。街道越窄，可以实现更高的截止高度角。然而，低仰角的nlos卫星可能会导致大部分gnss定位误差。即使对于具有挑战性的城市峡谷，提供本公开以检测和恢复信号遮挡和反射以实现高度准确的gnss定位。
[0070]
通过模型校准252进行的gnss nlos校正
[0071]
nlos校正通过基于swm的模型校准来执行。为了有效估计潜在的nlos误差，gnss接收器、卫星仰角和方位角之间的距离需要基于nlos误差模型。特别地，模型校准包括检测对应于nlos接收的反射点。传统上，光线追踪技术用于模拟nlos信号传输路径以找到nlos反射器。然而，这种技术具有计算能力高的缺点。本公开提供了一种不产生连续的建筑表面和清晰的建筑边界的方法。相反，swm仅提供大量密集、离散、无组织的点云。从swm中搜索反射器是使用基于反射器检测算法的有效kdtree结构执行的。
[0072]
用于执行模型校准的反射器检测算法的输入包括：卫星s的仰角历元t处的方位角以及方位角分辨率α
res
。输出是最近的反射点它是nlos卫星s最可能的反射面。
[0073]
在步骤1中，在3d lidar的中心初始化搜索点。搜索方向是根据卫星仰角和方位角确定的。
[0074]
对于典型的信号反射路径，包括两个段。第一段是从卫星到反射器的信号传输。第二段是从反射器到gnss接收器130的信号传输。由于反射信号应具有与预期定向信号相同的仰角，因此反射器检测算法的步骤2包括遍历从0
°
到360
°
的所有方位角，方位角分辨率为α
res
，仰角为从而找到所有可能的nlos传输路径。
[0075]
在步骤3中，如果与方向相关联的视距被点pj阻挡，则该点pj可能是反射点。特别是，如果连接点pj和卫星的视距没有被阻挡，则点pj被认为是可能的反射器并保存到
[0076]
在步骤4中，重复步骤2和3直到αs》360
°
，从而基于相同仰角的假设来识别所有可能的反射器。
[0077]
最后，步骤5基于最短距离假设，从以gnss接收器130和反射器之间的最短距离检测唯一反射器。
[0078]
有利地，反射器检测算法不依赖于建筑物表面的检测精度。短距离假设可以有效地防止过度校正，因为只有最近的反射器被识别为唯一的反射器。卫星s的潜在nlos延迟可以计算为：
[0079][0080]
其中，运算符||*||用于计算给定向量的范数。sec(*)表示正割函数。由于swm的稀疏性，虽然它仍然比3d实时点云更密集，但仍然存在一些使用swm无法找到反射器的卫星。因此，如果一颗卫星被归类为nlos，但在swm内部没有找到其反射点，则建议进行nlos重构。
[0081]
用于模型修复的nlos重构253
[0082]
具有较低仰角和较小snr的卫星具有较高的被nlos误差污染的可能性。传统的基于卫星仰角和信噪比的伪距不确定性建模方法可以在开阔地区产生令人满意的性能，但在城市峡谷深处却不行。特别地，加权方案以相同的方式处理los和nlos，这在已经检测到nlos时是不可取的。本公开通过执行nlos重构来校正gnss nlos接收，该重构实质上使用加权方案对los和nlos的不确定性进行建模，其中添加了比例因子以对nlos测量值进行去加权。加权方案包括：(1)如果卫星被归类为los测量值，则根据卫星snr和仰角计算比例因子；(2)如果卫星被归类为nlos测量值并且校正了伪距误差，则根据卫星snr和仰角计算比例因子；(3)如果卫星被归类为nlos测量值但未检测到反射点，则基于卫星snr和仰角以及比例因子kw计算比例因子。
[0083]
通过最小二乘算法的gnss定位254
[0084]
来自gnss接收器的伪距测量值表示为：
[0085][0086]
其中：
[0087]
是卫星和gnss接收器之间的几何范围；
[0088]
是电离层延迟距离；
[0089]
是对流层延迟距离；和
[0090]
是由多径效应、nlos接收、接收机噪声和天线相位相关噪声引起的噪声。
[0091]
同时，分别使用传统的saastamoinen模型和klobuchar模型来补偿大气效应(和)。来自给定卫星s的gnss伪距测量值的观测模型表示为：
[0092][0093][0094]
其中，是与相关的噪声。
[0095]
假设在用于进一步的gnss定位之前从中减去nlos误差观测函数的雅可比矩阵可以表示为：
[0096][0097]
其中，m表示历元t的卫星总数。
[0098]
gnss接收器的位置可以通过加权最小二乘迭代地估计如下：
[0099][0100]
其中，w
t
表示基于在nlos重构253中估计的权重的权重矩阵，如下所示：
[0101][0102]
其中，
[0103][0104]
被定义为基于卫星snr和仰角计算los测量值的权重如下：
[0105][0106]
其中，t表示信噪比阈值；a、a、f是预先确定的。
[0107]
图7为本发明第一实施例所开发的3d lidar辅助gnss实时动态差分定位方法应用前后的定位表现。传统gnss解决方案的经验有大约30米的误差，导致车辆在城市峡谷中行驶的车道误判。本改进的技术可以提供强大而精确的定位结果，以支持l4自动驾驶汽车的运行。
[0108]
实验结果
[0109]
为了验证所提出方法的有效性，在具有静态建筑物、树木和动态对象(例如双层巴士)的典型城市峡谷中进行了两个实验。第一个实验是在街道宽度为22米，建筑高度为35米的城市峡谷1中进行的。第二个实验是在街道宽度为12.1米，建筑高度为65米的城市峡谷2中进行的。
[0110]
车辆配备有u-blox m8tgnss接收器，用于以1hz的频率收集原始gnss测量值。3d lidar传感器是velodyne 32，配置为以10hz的频率收集原始3d点云。xsens ti-10 ins用于以100hz的频率收集数据。此外，novatel span-cpt是一种gnss(gps、glonass和北斗)rtk/ins(光纤陀螺仪，fog)综合导航系统，用于提供地面实况定位。fog的陀螺零偏运行稳定性为每小时1度，其随机游走为每小时0.067度。流动站与gnss基站之间的基线约为7公里。使用ros收集和同步所有数据。评估了以下五组配置：(1)仅u-blox gnss接收器；(2)加权最小二乘法(wls)；(3)排除所有nlos卫星的加权最小二乘法(wls-ne)；(4)借助对所有nlos卫星重新加权的加权最小二乘法(r-wls)；(5)加权最小二乘法，如果检测到反射器，则借助nlos校正，如果未检测到反射器，则重新加权nlos卫星(cr-wls)。
[0111][0112]
表1：城市峡谷1中gnssspp的定位性能
[0113]
使用五种方法的gnss定位实验的结果如上表1所示。使用u-blox接收器的定位结果平均误差为31.02米，标准差为37.69米。由于来自周围建筑物的严重多径和nlos接收，最大误差达到177.59米。对于wls，定位误差降至9.57米，标准偏差为7.32米。最大误差也降低到不到50米。排除所有探测到的非视距卫星后，定位误差增加到11.63米，比wls还要差。出现这种情况是因为过度的nlos排除会显著扭曲卫星的感知几何分布。可用性从100％略微下降到96.01％。因此
[0114][0115]
表2：城市峡谷2中gnssspp的定位性能
[0116]
在城市峡谷2更具挑战性的情况下，发现可能无法检测到由高于40米的建筑物反射的一些nlos卫星。使用u-blox接收器获得30.68米的定位误差，最大误差为92.32米。基于来自u-blox接收器的原始伪距测量值，使用wls获得了23.79米的gnss定位误差。与使用直接来自u-blox接收器的数据的gnss定位相比，最大误差略微增加至104.83米。从gnss定位(wls-ne)中排除所有检测到的nlos卫星后，均值和标准差分别增加到25.14米和23.73米。由于缺乏用于gnss定位的卫星，gnss定位数据的可用性下降到95.52％，这再次表明在城市峡谷中完全排除nlos并不可取。在nlos重构的帮助下，使用r-wls的2d误差降低到19.61米。保证百分百的可用性。使用cr-wls方法将gnss定位误差进一步降低到17.09米。结果的改进表明了所提出的3d lidar辅助gnss定位方法的有效性。最大误差仍达到71.28米，因为并非所有nlos卫星都可以被检测和缓解。
[0117]
图8是根据本公开的示例性实施例的车辆中的3d lidar辅助gnss nlos缓解方法的一种可能实现和结合的系统图。车辆使用lidar辅助gnss来使用卫星定位系统支持车辆进行定位。在某些实施例中，该系统包括3d lidar传感器110、ahrs 160、gnss接收器130、处理器810、存储器820、用户接口830、自主控制840和通信接口850。在某些实施例中，在不脱离本公开的范围和精神的情况下，3d lidar传感器110、ahrs160和/或gnss接收器130可以集成到处理器810中。在这种情况下，处理器810是具有能够接收gnss卫星信号、方向和/或
本地3d点云地图的内置组件的专用设备。
[0118]
处理器810可以是一个或多个通用处理器、专用处理器、数字信号处理芯片、专用集成电路(asic)或可被配置为执行以下一项或多项上述方法的其他处理结构。处理器810通信地连接到3d lidar传感器110、ahrs 160和gnss接收器130，用于分别接收局部地图、方向和gnss测量值。处理器810可以包括存储器820，或者与存储器820(作为分立组件)通信，用于存储定位数据和/或其他接收到的信号并检索有利于增强3d lidar传感器110的fov的swm的先前关键帧点云数据。
[0119]
在某些实施例中，存储器820可以包括但不限于固态存储设备，例如随机存取存储器(ram)和/或只读存储器(rom)，rom可以是可编程的、闪存可更新的和/或诸如此类的。这样的存储设备可以被配置为实现任何适当的数据存储，包括但不限于各种文件系统、数据库结构等。存储器820还可包括软件指令，其被配置为使处理器810执行根据本公开的方法一个或多个功能。因此，上述功能和方法可以被实现为可由处理器810执行的计算机代码和/或指令。然后，在一方面，这样的代码和/或指令可以用于配置和/或适配计算机(或其他计算机设备)根据所描述的方法执行一个或多个操作。存储器820因此可以包括具有嵌入其中的指令和/或计算机代码的非暂时性机器可读介质。计算机可读介质的常见形式包括例如硬盘、磁性或光学介质、ram、prom、eprom、flash-eprom、usb记忆棒、任何其他存储芯片或盒、载波，或计算机可以从中读取指令和/或代码的任何其他介质。
[0120]
用户界面830允许与驾驶员、乘客和/或其他控制车辆的个人进行交互，其可以包括选自由触摸屏、触摸板、按钮、开关、麦克风等所组成的组中的一个或多个输入设备。驾驶员或控制车辆的人可以使用用户界面830来激活或停用自主控制840，或选择不同的操作模式和/或自主驾驶的程度。
[0121]
自主控制840是用于基于gnss定位控制车辆的输出设备。处理器810因此可以确定车辆、静态建筑物和动态对象的位置，这可以使车辆能够相应地改变导航路线、速度、加速度、车载警报系统和/或其他功能。自主控制840因此可以实现智能交通系统的各种功能，例如自动驾驶、半自动驾驶、导航等。
[0122]
通信接口850提供允许数据与网络、车辆、定位服务器、服务器、无线接入点、其他计算机系统和/或本文描述的任何其他电子设备进行通信的接口。在某些实施例中，通信接口850可以包括但不限于调制解调器、网卡、红外通信设备、无线通信设备和/或芯片组(例如蓝牙设备、ieee 802.11设备、ieee 802.15.4设备、wifi设备、wimax设备、蜂窝通信设施等)等。
[0123]
该系统图旨在提供3d lidar辅助gnss的一般说明，而其他组件和系统块也可根据需要包括在内。系统内的一个或多个组件可以集成或分区，并且可以布置在车辆中的不同物理位置，或者以其他方式远程布置在服务器中或网络系统上。也可以提供超驰控制(未示出)或其他安全机制，以允许车辆的驾驶员或乘客在紧急情况下接管车辆的控制。
[0124]
如所证明的，本公开的方法提供了有效的方法来排除潜在的gnss nlos接收，借助3d lidar传感器和lio生成的环境描述，或使用3d lidar传感器和ahrs的swm。该方法即使在自动驾驶的城市峡谷中也能实现准确定位。本发明解决了定位精度不足阻碍自动驾驶应用部署的问题。在所有这些现有的定位解决方案之上，gnss-rtk是一种不可或缺的方法，能够在稀疏场景中提供全球参考定位。但是，由于信号遮挡和反射导致gnss nlos接收，在城
市峡谷中无法保证其准确性。本公开通过利用3d lidar的感知能力来检测和消除潜在的gnss nlos接收来解决这个问题。在某些实施例中，本文提供的方法即使在城市峡谷场景中也可以实现更高的精度，以满足自动驾驶的导航要求。
[0125]
上述方法可以直接应用于自动驾驶行业。具体而言，即使在城市峡谷中，本发明也可用于提供准确的定位解决方案。此外，该方法还可以应用于其他有导航需求的自主系统，例如移动机器人和无人机。同时，该方法还可用于城市峡谷的大地测绘。这说明了根据本公开的具有改进的定位性能的3d lidar辅助全球导航卫星系统。显然，上述公开的变体和其他特征和功能或其替代方案可以组合成许多其他不同的配置、设备、装置和系统。因此，本实施例在所有方面都被认为是说明性的而不是限制性的。本公开的范围由所附权利要求而不是由前面的描述来表示，并且旨在包含落入权利要求的等同意义和范围内的所有变化。
[0126]
缩写列表
[0127]
3d三维
[0128]
3dma3d地图构建辅助
[0129]
adas高级驾驶辅助系统
[0130]
adv自动驾驶车辆
[0131]
ahrs姿态航向参考系统
[0132]
dop精度衰减因子
[0133]
ecef地心地固坐标系
[0134]
enu东北天
[0135]
fov视场
[0136]
gnss全球导航卫星系统
[0137]
gnss-rtk全球导航卫星系统实时动态差分定位
[0138]
gps全球定位系统
[0139]
lidar光探测和测距(激光雷达)
[0140]
liolidar-惯性里程计
[0141]
imu惯性测量单元
[0142]
los视距
[0143]
nlos非视距
[0144]
pcm点云地图
[0145]
ros机器人操作系统
[0146]
rtk实时动态差分定位
[0147]
slam同步定位与地图构建
[0148]
swm滑动窗口地图
[0149]
tgnss历元
[0150]
gecef坐标系
[0151]
lenu坐标系
[0152]
s卫星索引
[0153]
rgnss接收器
[0154]
biahrs载体坐标系
[0155]
bllidar载体坐标系
[0156]
brgnss接收器载体坐标系
[0157]
ρ伪距
[0158]
历元t处的卫星的伪距
[0159]
历元t处的卫星的位置
[0160]
历元t处的卫星gnss接收器的位置
[0161]
δ
r，t
gnss接收器钟差
[0162]
卫星钟差
[0163]
信噪比(snr)
[0164]kw
用于加权的比例因子
[0165]
卫星仰角
[0166]
卫星方位角
[0167]
k搜索点索引
[0168]
nk相邻点数量