• apollo 自动驾驶中的GNSS/融合定位技术xiaoma_bk的博客-CSDN博客自动驾驶融合定位

    1 简介

  • 定位技术是自动驾驶技术解决方案中重要的一个部分，既有激光雷达、摄像头、超声波等其他技术的相对定位方式，也有GNSS高精度定位，多传感器融合定位。

  • 本文将为大家介绍「定位技术与自动驾驶关系」。包括以下内容：

    • 自动驾驶方案的构成
    • 自动驾驶为何需要定位
    • 高精度定位面临的挑战
    • 主要定位方式及特点
    • Apollo最新定位技术框架
    • 定位技术的离线应用
    • GNSS定位技术的基本原理
    • 定位误差源与差分RTK定位
    • 模型与RTK模糊度固定
    • 融合定位技术
    • GNSS/融合定位的挑战

      2 自动驾驶方案的构成

  • 目前主流的自动驾驶技术解决方案一般包括：高精度定位模块、感知模块和规划控制模块（就是PNC模块）。在这里把这三个过程和人类的驾驶行为过程做一个近似但不一定严密的对照。

  • 自动驾驶方案： | 人类驾驶 | 自动驾驶 | apollo技术方案 | | —- | —- | —- | | 定位 | 判断距离 | 高精定位 | | 感知 | 观察环境 | 环境感知 | | PNC | 决策行为 | 决策规划 |

  • 首先是定位。人类大脑提前存储了很多先验的地标或者路径信息，可以快速地判断当前的位置，也知道大概怎么去自己的目的地；对自动驾驶车辆而言，可能就需要依赖外部手段实现定位，目前技术解决方案包括：基于地图匹配、卫星定位等融合定位方式以及其它各种方式。

  • 其次是感知。人类通过双眼、耳朵观察周边；自动驾驶需要通过高精度地图、定位信息提供特定场景的静态结果，加上车端的实时计算感知到的动态环境，构成整体的周边场景的动静态结合的信息。

  • 最后是规划控制，也就是PNC。人类通过大脑做完规划和决策后，指令双手、双脚完成指令；自动驾驶汽车需要结合地图提供的参考规划结果+车端实时规划来完整地整体操作。

    3 自动驾驶为何需要定位

  • 人类是基于记忆与经验的基础，结合眼睛与耳朵收集信息，经过大脑判断决策后，指挥手脚完成具体的指令操作，实现整个驾驶行为。但是人去开车，也是有一定的门槛要求的。

  • 针对当前L4/L5级别（SAE定义的自动驾驶的级别）自动驾驶为什么需要精确的定位系统？这里举一个具体的例子来进行说明。
    • 对于人类开车，大家会关注什么？可能会关注车道线、道路指示牌、红绿灯这些交通标识，以及路边马路牙子、路边柱子，建筑物，这些都是场景中的静态物体，关注它们是因为人需要知道车的可行使范围以及需要规划车的行驶路径，并且遵守相关的交通规则。与此同时，我们还会关注一些动态的物体，比如对面开过来的摩托车。
    • 对于无人驾驶汽车，也需要关注静态场景和动态物体。静态场景的车道线、红绿灯等信息是通过高精地图和精确定位来实现的；动态物体检测是通过感知算法来完成的。

  • 那么，定位在自动驾驶中的作用有以下三点：

    • 第一，与高精地图配合提供静态场景感知
    • 第二，感知得到的动态物体正确放入静态场景
    • 第三，位置和姿态用于路径规划和车辆控制

      4 高精度定位面临的挑战

  • 定位本身需求：

    • 高精度：需要达到厘米级的位置精度才能和HDMAP匹配使用，只有姿态和位置足够准了，红绿灯的映射才能在相机中找到较为合理的位置；
    • 高可用：可能需要所有的场景，或者说应该争取做到所有的场景，整个定位服务都是可用状态；
    • 高鲁棒：我们希望定位的解是高度可靠的，自带的置信度能够充分反映解的水平。最终，在定位整体由于某些故障，能够及时报警自身不可用的状态。
    • 人们常说GPS定位技术，以及后面会提到的RTK定位技术可以达到厘米级，足够精确，但是GNSS信号本身特别容易受到信号遮挡影响，地库、树下等场景基本不可行。
    • 我们会使用一些基于地图匹配的方式（比如LiDAR和相机）来实现定位，但是这些匹配在一些恶略天气、或者地面修葺后环境变更的场景，可能也有一定的难度等等。

      5 主要定位方式及特点

  • 目前的定位方式主要包括：卫星定位，使用图像/LiDAR进行map-match的绝对定位和 slam类似的增量定位，惯性导航和轮速计等。

    • 卫星定位：是一种绝对定位方式，通过一定的增强手段，可以实现全天候（和时间/气候不相关）厘米级定位，但是容易受遮挡、电磁干扰，比较脆弱。
    • 激光雷达/相机和提前建好的地图做地图匹配map-match：据目前最新的一些论文看，精度和鲁棒性都算可以，但是在提前建设的地图比较大，基建狂魔下的中国，环境变更也特别多，另外对于相机而言，光照变化、相机遮挡也是一个较大的挑战。
    • 惯性导航和轮速计：优点就是无外部依赖，但是整体随着时间积累误差积累较快。

  • 定位技术的特点 | 卫星定位 | 激光雷达 | 视觉 | 惯导 | 轮速计 | | —- | —- | —- | —- | —- | | 定位模式 | 绝对定位 | 绝对，有地图 相对，无地图 | 绝对，有地图 相对，无地图 | 相对 | | 误差增长 | O(1) | O(1)，有地图 O(T)，无地图 | O(1)，有地图 O(T)，无地图 | O(T^2) | | 缺陷 | 信号阻挡、多径、电磁干扰 | 施工环境变化、地图大 | 施工环境变化、光照变化、遮挡等 | 累计误差大 | | 优点 | 全天候、高精度 | 高精度 | 低成本 | 无外部依赖 |

  • 这些传感器的定位方式，都有各自的优缺点，想要独立去实现所有场景下的高精度、高可靠定位能力几乎是不可能的，目前较好的解决方案，就是希望各种方式都挖掘最大的潜力，然后互相弥补、备份，在统一融合解的层面，去追求精度、可用、可靠等核心指标。

  • 为了更好地介绍GNSS和融合定位，我们简单介绍一下LiDAR和视觉DA4AD定位算法。

    5.1 LiDAR定位

  • 激光雷达定位，主要就是在线点云与预制底图，基于预测pose在小的搜索范围内，按照滑动像素的方式，计算features（特征）之间的匹配度cost value，然后把cost value换算成概率，最终根据概率分布来进行位置计算。这里的features可以是反射值或者高度等信息，具体细节可以参考团队万博士在2018年ICRA的论文或者斯坦福大学塞巴斯蒂安团队的论文。

  • 激光定位：实时激光点云+预先建立的地图 -> 最有匹配

    5.2 视觉定位

  • 目前百度也在内部上线了视觉定位技术。部分场景下，环境中的杆状物相对比较稳定，比如车道两边的一些路面可能因施工导致反射值变化，但是两边的一些树干、灯杆变化较少。基于这些杆状物的定位可以提供不错的稳定性。

image.png

  • DA4AD方案具体说就是离线制图训练时，通过LFE（局部特征提取模块）提取图像特征和描述子，再与3D点云关联，相当于选取了3D中的某些点，这些点都绑定了一些特征与描述子，将这些3D点及其特征与描述子写入文件，形成视觉定位地图。

  • 我们在线定位时，通过predictive pose（融合积分给出的概略位置）查询对应临近的map key points，与online image的特征/描述子之间构建匹配关系，最终获得最优pose。整个过程都是通过神经网络完成。

    6 Apollo最新定位技术框架

    Apollo最新定位技术框架，主要包括以下几个部分：

  • 输入：轮速计/接收机/LiDAR/相机和IMU；

  • 输出：位置/姿态/速度和对应置信度等信息；
  • 用户：主要是PNC和感知模块；
  • 定位内部：包括RTK算法、LiDAR匹配、视觉匹配模块以及SINS编排和融合模块。

image.png

7 定位技术的离线应用

  • 关于GNSS/融合定位技术的一些应用，上文提到，我们的感知、规划控制，甚至我们的定位需要高精度地图的部分支持。下面简单介绍一下整体离线下高精度地图的生产流程。image.png
  • 主要包括：采集车上路道路采集、内业数据处理（GNSS/IMU的后处理提供初值），完成点云拼接和底图生成，之后就是一些基于深度学习的元素识别，人工质检，到最后的产品编译。

  • 其中，针对于很多或者说大部分场景下，GNSS的可用性较高，因此GNSS+IMU离线后处理作为初值，能够极大地提高我们大规模建图的效率，是整个流程的关键输入之一。

    8 GNSS定位技术的基本原理

    关于GNSS定位技术的基本原理：

  • 用户设备可以从接收信号中解码获得数据码，包含电文内容。这样，卫星轨道参数已知，通过时间计算卫星的位置、速度和钟差；

  • 从卫星到地面的传播距离通过测距码信号处理获得传播时间后，乘以光速得到距离；
  • 通过3颗卫星交会，可以得到用户位置；
  • 但是刚才提到的测距传播时间包含本地接收机的误差（本地接收机时钟精度差一些），需要额外增加一个参数来估计。

GNSS 定位技术：

  • 距离教会定位原理

    • 电文已知
      • 轨道参数，加上若干摄动项，根据时间可以算出卫星的位置
    • 距离交会
      • TOA测距，3颗卫星相交交会2点，舍弃外部空间
    • 需要4颗
      • 本地钟差较大，设参估计
      • 以4颗卫星观测值估计：XYZ + 钟差dt，来实现定位、授时。

        9 定位误差差源与差分RTK定位

  • 实际上卫星位置和测距都包含较大的误差，导致这个用户计算的位置也有一定的误差。具体包括：

    • 起算点基准、卫星端：位置误差、卫星钟差（虽然卫星钟作为原子钟具有很强稳定性，但是也会有慢飘后）；
    • 信号传播路径上：包括电离层延迟、对流程延迟、多路径效应等；
    • 用户接收设备上，可以包括天线相位中心偏移与稳定度、接收机钟差与信号处理噪声等。
  • GNSS定位技术

image.png

  • 以上误差的累积会导致用户定位最终出现偏差。目前各大卫星导航系统的SPS（标准定位服务）一般宣称2 sigma 5 ～ 10 m 。上个世纪80年代末/90年代初，GPS上线不久后就有学者提出RTK技术概念了。我们这里就聚焦于RTK。
  • Real Time Kinematic，RTK，在用户Rover端和基站端Baser（距离一般短于30km）构建差分观测值，消除或者削弱包括卫星端误差、传播路径误差、用户端误差等，在模糊度正确固定的前提下，实现厘米级定位。用户和基站需要足够“近”，就是为了保证与二者之间到卫星的视径足够相似，各项误差相关性足够强，认为基本等价。
  • 关于伪距和载波。目前接收机处理的信号一般包括载波、测距码和数字码。数字码解码出来就是前面提到的很多电文信息，测距码的就是通过窄相关等技术可以获得等效的地卫距，目前号称0.3m～0.5m左右的精度；再剥离测距码和数据码之后就剩下载波。

  • 用户设备的环路一般都能够获得精准相位差的相位差，精度在2mm左右，但是却无法知道从卫星到地面有多少个cycles，就是有一个未知整周在里面。所以，即使消除所有误差、用伪距，精度也无法达到10cm以内，用载波的话，就必须求解模糊度，也就是RTK提到的模糊度正确固定前提的原因。

    10 模型与RTK模糊度固定

  • 具体的数学模型过程包括：

    • 在伪距/载波量测上，构建单差方程；
    • 法方程求解；
    • 获得单差模糊度及因数阵等；
    • 因为单差模糊度受到接收机钟差影响，具有非整数的特性，我们一般在各个卫星系统内选取参考卫星，构建转换矩阵，然后可以获得双差模糊度和双差因数矩阵等。
  • RTK双差模糊度参数是整数解，直接通过整数最小二乘的方式无法获得解析解。目前，通用的方式就是通过搜索的方式来实现，包括坐标域和模糊度域等方式。image.png
  • 相对来说LAMBDA、MLAMBDA、Lattice理论等方式在业内使用比较多。只有正确固定的模糊度下，RTK才能达到厘米级解。所以，整个核心就是在于：整周模糊度的固定和确认。
  • 那么如何确保 LAMBDA、MLAMBDA等方法搜索出来的模糊度是正确的呢？目前的话，一般分为2个步骤，就是解的 模型强度验证（model driven）和解的数值验证（data driver）。
  • 模型强度验证（model driven） 指的是：在使用MLAMBDA等搜索时，是需要对输入的高度强相关的协方差矩阵进行降相关处理的。我们可以对对角线上元素对应的模糊度进行逐一的取整处理，得到对应的整数模糊度，然后对应的Qzz（i，i）表征了该整数解的精度。通过理论分析，可以把整个过程的最小成功率，通过累积乘得到，获得total-s，整个解的最低成功率，对应的最高失败率就是1- total-s。
  • 实际应用中，可以要求积累足够数据后，解的强度，也就是最大失败率低于一定的门限时，才尝试固定模糊度。
  • 假设卫星的载波数据是包含半周跳的（信号处理机制导致载波非整周），即使强度非常强，任何搜索出来的整数解应该是有问题的。因此，我们需要从数据层面去检验整数解的正确性。
  • 目前业内有包括ratio假设检验、difference距离假设检验等验证方式。

image.png

  • 我们先构造一个整数解与浮点解的马氏距离，作为我们的残差评价，在次最小/最小值构造比值，进行F分布假设检验。C值的选取，一般和当前的失败率与观测值个数相关，Verhagen/Teunissen团队也有相关的论文阐述该问题。当失败率较高、观测值个数较低时，一般c值会更大，反之亦然。那么在具体工程话实践中，我们该如何选取这些值呢？
  • 超参数设置？数据驱动下的测量参数装订与自动化数据回归

image.png

  • 刚开始会有一些基础数据集，支持我们的研发；有大量的数据基础上，通过效果分析去评估RTK固定的正确率和错误率，然后逐步调整；上线后，我们有车队跑的数据，反过来可以通过和其它传感器数据一起分析后，挖掘出来一些RTK的bad cases，继续添加到我们的数据库，进一步优化我们的参数。
  • 即使我们通过大数据，慢慢找到了合理的门限来进行确认性架设检验，但是实际过程当中，我们会遇到这种情况，卫星观测值较多，整个观测值强度非常强，失败率非常低，但是如果载波数据受到一些bias影响，会导致基于data-driven的一些检验无法通过，迟迟进不了固定解。例如我们知道GLONASS FDMA的体制导致目前市场上不同接收机在GLONASS载波观测值上有一个IFB（rate）值。
  • 如果初值给的不够准确，就会导致部分模糊度的浮点解有偏差，那么整体类似ratio检验很难通过。另外，即使初值足够准确，在类似观测值中有个别半周跳，或者未检测修复情况下，浮点解可能也会有Bias。
  • 但大部分观测值还是正常的。这时就需要我们按照某种策略，尝试固定其中部分有效模糊度；从而整体提高RTK的固定率和固定速度。

  • 选取Subset的方法包括：如收到GLONASS的影响先不选取GLONASS的观测数据，而是先把GPS、伽利略、北斗的数据考虑进去；如果担心观测值受影响，是不是考虑高仰角的卫星，信号比较高的卫星，其质量比较有保障；最后的解算模型中要先考虑失败率比较低的；在浮点解的时候，计算每个模糊度的残差，残差小的是不是需要优先组合出来。这些都是可以在工程实践中尝试使用的，当然也有其他的一些方式。

    11 定位融合技术

    11.1 在线融合定位

  • 在线融合定位上下游

image.png

  • 融合定位，主要输入就是IMU、轮速计和各个量测传感器的量测输入，输出包括给下游用户的感知和PNC模块。同时，也会将IMU的实时积分结果给到RTK引擎和LiDAR、视觉定位模块，这些量测子模块基于先验输入可以做一些预处理操作等。

    11.2 后续处理融合定位技术

  • 除了在线部分，融合定位技术也应用于我们离线的高精地图制作过程。

image.png

  • 如图所示，左边的流水线图，是我们高精度地图的生产与验证流程，主要包括： 主道路采集、Pose后处理解算、点云拼接优化、底图生成与质检、HDMAP标注、仿真与测试等过程，最终发布使用。

  • 右边则是整体的后处理的流程。我们拿到Record数据后，RTK引擎基于Obs原始数据和Rtk-eph的星历数据完成RTK解算，SINS engine基于IMU数据完成捷联惯导编排，和轮速计一起，分别输入到前向和后向滤波中，前向和后向滤波的解，进入一个平滑滤波器，最终得到相关的结果。

    11.3 SINS的捷联编排

  • 下是SINS的捷联编排的基本数学模型和实际的积分实现过程。

image.png

  • 左边是当N系下的运动学模型，姿态的变化率和当前姿态与角速率相关，速度变化率应该在载体加速度姿态转换后，还需要考虑有害加速度和重力加速度的影响等。

  • 右边是具体的积分过程。绿色框是线上IMU输出的IMU坐标系下的加速度和角速率，通过零偏补偿后，再通过姿态转换到N系下，接着进行地球自转/位移角速率的补偿后，完成姿态的积分，然后进行速度积分，最后基于位移角速率完成位置积分。这就形成一个完整IMU周期内的姿态、速度和位移积分过程。

    11.4 融合定位模型与流程

  • 目前，我们的融合定位函数选择了当地导航坐标系下的小角度误差模型作为基础。

  • 具体而言，我们以位置、姿态、速度的误差作为状态量，那么误差的变化率和误差本身在小角度误差假设前提下，形成了一个线性微分方程。基于此，我们在连续时间上，可以在t和t+delta-t上构成一个线性系统，离散化后，可以形成前向和后向的离散化方程。右边是一个常规的融合滤波的数据流。我们在收到IMU数据后，我们会先判断滤波是否完成位置初始化、水平对准和航向初始对准；完成较为准确的航向对准后，可以进入SINS积分周期，而后进入卡尔曼滤波的时间更新，如果存在有效的RTK或者视觉和LiDAR、轮速计等量测时，进行量测更新。
  • 融合定位的注意点在融合定位工程化过程中，应该稍加注意的技术点。第一个是多个传感器量测延迟不一致的问题。
    • 在实践的时候，因为LiDAR点云处理相对来说时间上比较长，GNSS定位计算比较简单，实际给个参考值lidar大概处理时间是100ms左右，gnss在0.2ms左右。Tc是我们量测数据处理完毕，发送给融合滤波的时间。这里有个延迟就是 tb时间线上更晚来的GNSS数据t2，做完计算后比更早的t1-lidar数据发送到融合滤波。于是就设计并行滤波器，filter1和 filter2。
    • 第二个就是传感器量测的粗差探测与隔离工作。前面我们提到，我们希望多传感器来一起实现定位的高精度/高可用和高可靠性。但每个传感器都有自己的特点，都会有一些野值outlier产生，融合要想稳定的工作，就需要有效识别并隔离这些outlier。
  • 目前有一些比较好的方法，包括：
    • 单sensor的新息卡方检验：就是新的量测和预测pose之间构建新息向量，并和因素阵做归一化后，构成一个卡方分布，我们基于漏检、误检需求，构建卡方检测；
    • 单sensor的新息观测值，在时间维度上，应该是存在较强的相关性的，如果在时间轴上的某一点发生突变，那么极有可能存在一些outlier引入；
    • 最后，就是多个sensors之间的一致性检验。

  • 传感器野值探测、隔离 | Sensor | 异常例子 | 解决方案 | | —- | —- | —- | | IMU | 不输出 输出异常 | 1.直接退出工作状态？ 备份IMU+实时检测，并行滤波等 | | GNSS | 模糊固定错误 | 卡方检验，信息序列检测，交叉检校 | | idar | 局部最优（异常值） | 卡方检验，信息序列检测，交叉检校 | | DA4AD | 局部最优（异常值） | 卡方检验，信息序列检测，交叉检校 |

  • 上图表格是我们实际会遇到的部分问题。

    • 比如IMU，会因为硬件、软件原因不输出，那么我们的策略是直接退出工作状态，还是我们从硬件上做好备份，然后做好在线监控和并行滤波，随时准备切换，这些都需要我们进行思考。
    • 而GNSS，典型就是RTK固定错误；`
    • LiDAR和DA4AD的话，作为Map-match`的方式，总会存在一些场景，存在局部最优的情况。这些情况，都可以通过前面说的卡方检验、新息序列检测、交叉检校， 3种方式进行探测与隔离，保障融合本身的正确性。

有效数据闭环

  • 有效数据闭环示意图

image.png

  • 在基本的技术框架和工程实践实现之后，数据仍然是我们去优化整体融合定位表现的核心要素之一。
  • 具体来说，有一部分基础数据raw data，去支撑我们算法研发和调试，基于和真值的对比评估，我们会调整我们的参数（包括IMU的噪声参数）和一些具体的策略（包括卡方检测的一些门限）等。在达到一定的水平后，我们更新上线到车辆。这些车辆产生大量数据，我们做一些有价值的数据挖掘，再加入到我们的benchmark当中，最终形成一个闭环，完成Corner cases收集及自动化数据回归。
  • 这里给出一组我们这个流水线自动召回的一个有效case。

  • 车辆在测试的时候，可能我们的安全员司机是在手动开车的，那么可能这个时候，他没有太关注相关的一些GUI提示界面，这样在一些短暂的定位漂移的时候，刚好错过了，可能这个bug就漏掉了。但是数据回来后，我们会例行跑，就会根据一些逻辑召回出这个case。

    11.5 离线后处理解算

  • 融合定位后处理本身框架和在线基本上是一致的，同时我们有一个后向解，其实是可以从GNSS固定层面上去提供一个新的尝试。在很多具体的场景下，去前向跑一个RTK固定解的话，可能因为策略设置问题而没有固定，但是如果我们能够从后向去跑一下的话，这些数据段可能是能够固定的。我们整个GNSS的固定的解比例是会有一定的提升的。

  • 基于后处理的结果，去完成点云拼接。可以看到，整个建筑物的边缘都非常清晰，车道线，还有杆状物的信息也非常清楚。这证明我们给出的位置精度和姿态精度是能够满足做点拼接需求。

    12 GNSS/融合定位的挑战

    最后我们探讨一下GNSS定位技术和融合定位技术的一些挑战，希望大家一起推动技术问题的解决和优化。

  • GNSS本身完好性检测？RTK的可靠性、可用性均衡？

  • 融合定位完好性检测、自动驾驶下定位完好性体系论证？
  • 行业共建GNSS/融合定位开源数据集？