任务5.3 融合定位算法-学生手册

任务5.3融合定位算法学生手册任务5.3融合定位算法【任务导入】智能驾驶场景繁多、定位精度和稳定性要求高，传统的定位技术已不能够应对所有场景，融合定位技术用于提供智能驾驶全场景的定位方案。那融合定位技术包含了什么算法？又有哪些具体用途呢？【学习目标】素质目标通过融合定位技术的通用框架和技术栈，培养学生的综合分析能力和沟通表达能力；通过融合定位各技术模块的算法原理，激发学生的探索精神。知识目标能说出融合定位技术在智能驾驶中的作用[K73]；能列举业界融合定位技术的通用框架和技术栈[K74]；能描述融合定位各技术模块的算法原理[K75]。能力目标能指定一套具体的融合定位方案[A61]；能使用融合定位算法进行融合定位[A62]。

【知识准备】融合定位技术定位技术在智能驾驶中的作用在智能驾驶应用中，感知，融合，规控算法是强依赖于定位技术的，在各个算法模块中，定位占据非常重要的位置，如图5-3-1所示。定位是所有其他功能的前提，定位的丢失以及错误的定位将会使得高阶段的智能驾驶汽车无法运行，酿成重大事故。量产化车载定位要求：全天候全路况覆盖。图5-3-1定位在智能驾驶中的位置在定位模块中最主要的的输出是定位信息，包括世界坐标系下的位置信息，x，y，z以及俯仰角（Pitch）、偏航角（Yaw）、翻滚角（Roll）的姿态信息，一般情况下，定位模块还包含IMU模块，所以速度和加速度信息一般也会在定位模块中输出，如图5-3-2所示。图5-3-2姿态信息定位技术存在的问题目前定位技术在现实复杂工况下还存在诸多问题和难点，包括：在复杂的高架桥下，GNSS（GlobalNavigationSatelliteSystem，全球卫星导航系统）信号十分不稳定，RTK（Real-timekinematicpositioning，实时动态载波相位差分技术）不能正常定位，误差很大，如图5-3-3所示。图5-3-3复杂路况树荫的遮挡等环境，GNSS信号微弱，导致信号异常，如图5-3-4所示。图5-3-4树荫遮挡高层建筑物等密布区域、湖边、海边，GNSS信号容易反射，出现多路径效应，定位误差很大，如图5-3-5所示。图5-3-5密布区域遮挡环境下GNSS与IMU（Inertialmeasurementunit，惯性测量单元）的融合：长时间进入隧道、地下停车场等无GPS信号的环境下，IMU航迹推算存在累计误差，目前亚米级的定位精度（测量精度达到米以下的分辨率）也只能保持数十秒的定位时间，远不能满足智能驾驶的要求，如图5-3-6所示。图5-3-6遮挡环境在高速动态场景下，由于速度快，当输出频率无法满足时，RTK的定位精度也会下降，如图5-3-7所示。图5-3-7高速动态场景定位技术现状单功能定位技术业界针对以上的问题和难点展开了许多研究，常见的单功能定位技术见表5-3-1。表5-3-1单功能定位技术定位技术名称方法简介优势不足GPS、GNSS-RTK定位基于信号的定位（如GPS、GNSS-RTK定位）。可实现全球、全天候、全天时定位，绝对位置准确。依赖卫星信号，在收到遮挡时，信号丢失；容易收到电磁环境干扰。IMU定位航迹推算定位（依靠IMU）等，根据上一时刻的位置和方位推断现在的位置和方位。不依赖于任何外部信息，不受外界电磁干扰的影响。数据更新频率高。存在累计误差里程计定位基于激光雷达/立体视觉里程计定位，用观测到的前后帧或左右图像/激光数据进行匹配，得到现在车的位置和姿态。可获得周围环境的3D信息。特征受环境影响较大，且特征不具有识别性。融合定位技术单功能定位技术虽然一定程度解决了定位问题，但是仍然无法有效的解决复杂的定位问题，保证定位精度，所以业界引入了融合定位模式，主要通过融合各项单功能定位结果弥补各自缺点，实现稳定的定位输出，目前业界常见的定位方法有三种模式，见表5-3-2。表5-3-2融合定位技术定位技术名称方法简介优势不足GPS+IMU融合定位IMU进行推算，GPS进行定位约束。结合IMU定位，可以有效解决GPS受到外界环境干扰的问题。在长时间GPS信号丢失时，IMU会产生累计误差导致定位不准。（如隧道、地下停车场场景）。GPS+IMU+里程计融合定位IMU进行推算，GPS以及视觉/激光里程计进行定位约束。在长时间GPS信号丢失时，可以通过视觉（激光）里程计修正IMU的累计误差。视觉（激光）里程计容易受到光线、环境等影响，在推理反算过程中会产生误差。GPS+IMU+里程计+高精度地图融合定位视觉定位技术高精度地图特征准确可近似为真值，有效地抑制了视觉或激光受环境光照导致特征不准确的影响。其计算效率较大，而且初始定位位姿值直接影响整体的匹配效率。激光定位技术融合定位方案概览目前业界内比较通用的融合定位整体架构，共5个模块：定位传感器模块、里程计模块、融合模块、匹配模块、SLAM建图模块，如图5-3-8所示。5个模块可以进行任意的组合，从而实现一个稳定的融合定位方案。定位传感器模块：主要包含GPS和IMU，也可以是GPS和IMU的组合定位；里程计模块：业界比较通用的方法是轮式里程计、视觉里程计、激光里程计；融合模块：主要分为两类，基于滤波的方法，包括扩展卡尔曼滤波、误差状态扩展卡尔曼滤波，无迹卡尔曼滤波；基于优化的方法，如图优化等；匹配模块：特指定位信息与地图的匹配，根据不同的地图类型，适配不同的匹配方法；SLAM建图模块：主要采用视觉或激光SLAM定位建图方案进行建图定位（此处的SLAM模块特质建图部分），完整的SLAM方案同样包含前端里程计和后端优化中。图5-3-8融合定位整体框架融合定位算法模块介绍融合算法融合算法的典型算法为卡尔曼滤波系列，其主要分为两个过程预测过程和更新过程。预测过程：根据当前的输入信息，以及预测模型方程输出预测结果；更新过程：根据当前观察到的数据信息，和之前的预测信息进行比较，再反向更新预测模型方程。以车辆定位为例，当获取当前时刻的速度和加速度信息，就可以根据车辆自身运动方程预测一段时间后车辆的位置，但是在真实场景下，传感器数据一般存在误差，因此还需要增加误差项，也就是方差。如图5-3-9所示。图5-3-9预测过程当有多种传感器，如GPS，IMU，里程计等输入数据时，每个传感器都可以预测下一状态位置，因此可以通过优势互补，如图5-3-10所示。图5-3-10多传感器预测根据不同来源预测位置的可靠度进行加权平均，从而获得融合后的结果，如图5-3-11红色车辆位置。图5-3-11加权平均获得融合结果里程计算法视觉里程计以单目视觉里程计为例，其输入主要为单目摄像头图像，IMU数据，输出为定位信息，整体过程大致可以分为前端和后端优化，在前端数据输入至后端处理时，需要利用IMU预积分做初始化判断，对滑动窗口内所有人的位姿和特征点进行深度求解。此外还有局部地图模块，用以保存滑动窗口类的特征点和关键帧信息，如图5-3-12所示。图5-3-12视觉里程计在前端过程中，主要为图像特征点检测、跟踪模块以及IMU预测模块，如图5-3-13所示；

图5-3-13图像特征点提取在后端优化过程中，接受不同时刻的特征匹配信息和IMU测量信息，结合滑动窗口局部地图，构建图模型对它们进行非线性优化，从而得到局部一致的位置信息输出最终的定位结果，如图5-3-14所示。图5-3-14特征点匹配激光里程计激光里程计与视觉里程计类似，其输入数据为激光雷达的点云数据和IMU数据，输出为定位信息，同样分为前端和后端优化过程。但由于激光雷达在运动过程中，数据会产生畸变，所以还需要去除畸变。畸变形成原因：由于车辆的运动，激光束的距离数据都是在不同位姿下采集而来；然而，一般激光雷达驱动封装数据时，却默认一帧激光雷达数据的所有激光束是在同一位姿下、瞬时获得的，也就是所有激光束都是从运动初始位置获得的数据，这样实际使用的数据和真实数据有明显差距。去畸变原理：在一帧激光雷达数据中，为每个激光束的原点用IMU找到一个近似的里程计位姿，进行系列的坐标变换，把所有激光点的数据都变换到第一束激光原点的坐标系下。在前端过程中，主要利用IMU对测量点云数据进行去畸变，再通过点云匹配算法计算相邻帧相对运动。局部地图中主要包含滑动窗口类的激光特征和关键帧信息，激光特征主要包括点特征、线特征以及面特征。再后端优化过程与视觉里程计类似，接受不同时刻的特征信息和位置信息，结合局部地图进行非线性优化，从而得到局部一致的位置信息，并输出位置结果。SLAM算法SLAM（Simultaneouslocalizationandmapping，同时定位与地图构建）算法和里程计算法类似，相比于里程计算法，主要增加了回环测试和建图模块，其主要应用有视觉SLAM和激光SLAM，由于处理的流程类似，下面仅对视觉SLAM进行分析。视觉SLAM经典视觉SLAM框架由5部分组成：传感器数据读取、前端视觉里程计、后端非线性优化、回环检测和建图，如图5-3-15所示。传感器数据读取：在视觉SLAM中主要为相机图像信息的读取和预处理。视觉里程计（VisualOdometry，VO）：视觉里程计的任务是估算相邻图图像间相机的运动，以及局部地图的样子。VO又称为前端（FrontEnd）。后端优化（Optimization）：后端接受不同时刻视觉里程计测量的相机位姿以及回环检测的信息，对他们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端（BackEnd）。回环检测（LoopClosing）：回环检测判断机器人是否到达过先前的位置。如果检测到回环，他会把信息提供给后端进行处理。建图（Mapping）：根据估计的轨迹，建立与任务要求对应的地图。图5-3-15视觉SLAM处理流程高精度地图高精度地图（HDMap，HighDefinitionMap）是专门为智能驾驶服务的地图。高精地图由于其要素丰富、精度较高，可以提供车道级别的导航，以及高精度定位，如图5-3-16所示。图5-3-16高精度地图应用场景辅助环境感知；高精度定位辅助；路径规划与决策的信息来源。地图格式NDS（TheNavigationDataStandard，导航数据标准）：在兼顾性能和功能的基础上，采用了数据库技术存储地图数据，能够比较好地解决地图增量更新、扩展和数据安全的问题。这种规格的价值在于全球规格、地图切换、增量更新、云混合、高精度地图标准等。Opendrive：使用XML语言描述，定义了Road、Junction、Station等数据结构，该规格地图较为灵活，业界使用比较广泛。地图匹配算法业界比较通用的地图匹配方法为ICP和NTT匹配算法。ICP（最近邻迭代算法）ICP（IterativeClosestPoint）最近邻迭代算法，该算法输入两组点云数据及迭代次数及误差阈值，输出为点云之间变换关系的旋转R和平移T矩阵，如图5-3-17所示。图5-3-17ICP最近邻迭代算法算法流程①在目标点云P中选取点集𝑝𝑖，选取点集方法：可以采用均匀采集、随机采集等；②在高精度地图点云Q中选取对应的点集𝑞𝑖，𝑞𝑖的选取是与𝑝𝑖点集距离最近的点集，即所有点欧式距离和最小，其中选取对应点方法可以采用点到点，投影或者面到面距离进行确定；③计算旋转R矩阵和平移T矩阵，使得误差函数最小，计算变换矩阵方法一般采用SVD奇异值分解法；④经过步骤③的旋转R矩阵和平移T矩阵后得到新的点击𝑝𝑖’：𝑝𝑖’=𝑅∗𝑝𝑖+𝑇⑤计算𝑝𝑖’到点集𝑞𝑖的平均距离d；⑥如果d小于预设的阈值或者超过了迭代的次数，则停止，否则跳到步骤②，直到满足收敛条件。ICP的优缺点优点：简单且计算复杂度低，目前较为通用的匹配方法。缺点：容易陷入局部最大值，两个点云的初始位置必须足够近，并且当存在噪声点、外点时可能导致配准失败。NDT（正态分布变换算法）NDT（NormalDistributionsTransform）正态分布变换算法，该算法输入两组点云数据，输出旋转R和平移T矩阵。其目标是找到当前扫描的姿态，使得当前扫描的点位于高精度地图表面上的可能性最大化。算法流程①将空间划分成各个格子Cell；②将点云所有点投票到各个格子中；③计算格子的正态分布PDF参数，即点中心和协方差，PDF可以看成局部表面的近似值，描述了表面的位置以及它的方向和平滑度；④将第二幅扫描点云的每个点按转移矩阵T的变换；⑤第二幅扫描点落于参考帧点云的哪个格子，计算响应的概率分布函数；⑥当前扫描的点云估计的旋转和平移向量。采用牛顿法求所有点的最优值，NDT优缺点优点：基于概率统计，整体分布，可冗余微小变化，计算量小。缺点：格子大小设定需要经验值，格子中低于五个点对结果影响较大。

【任务实施】概述本实训任务的将基于c++和python语言实现视觉里程计算法的复现，并且根据算法接口，分别测试单目摄像头与深度摄像头的数据集，生成对应的标注文件，测试相应的视觉里程计算法效果。实验环境准备设备检查本次实验环境包括电源线，安装Ubuntu20.04虚拟环境的PC机一台，见表5-3-3。每套实验环境适用于4~12名学生同时上机操作。表2-3-1实验清单设备名称数量备注电源线1组所有实验组共用笔记本或台式机每组1台台式机要有无线网卡项目描述视觉里程计项目文件：ORB_SLAM2文件夹，如图5-3-18所示。图5-3-18项目描述数据集现有数据集：rgbd_dataset_freiburg1_room、rgbd_dataset_freiburg2_360_kidnap本任务采用TUM数据集，TUM数据下载地址：https://cvg.cit.tum.de/data/datasets/rgbd-dataset/download视觉里程计算法复现运行单目摄像头视觉里程计算法打开项目文件夹ORB_SLAM2，右键鼠标打开终端框，输入以下命令：./Examples/Monocular/mono_tumVocabulary/ORBvoc.txtExamples/Monocular/TUM1.yamlrgbd_dataset_freiburg1_room/运行程序后，出现单目摄像头的视觉里程计，如图5-3-19所示。图5-3-19单目摄像头视觉里程计修改视觉里程计数据集将原本单目摄像头数据集修改为深度摄像头数据集rgbd_dataset_freiburg2_360_kidnap，运行以下命令，生成图像RGB信息和深度信息depth的关系，如图5-3-20所示。python3associate.pyrgbd_datasetfreiburg2_360_kidnap/rgb.txtrgbd_dataset_freiburg2_360_kidnap/depth.txt>rgbd_dataset_fr