无人驾驶实战-第八课（定位算法）

在七月算法上报了《无人驾驶实战》课程，老师讲的真好。好记性不如烂笔头，记录一下学习内容。 课程入口，感兴趣的也可以跟着学一下。

—————————————————————————————————————————

无人驾驶中定位的作用：

定位+高精度地图：提供当前位置的静态环境感知 （车道线/交通指示牌/红绿灯/柱子/建筑物/等）

定位+动态物体感知：将感知到的动态物体正确放入静态环境 

定位获取位置姿态：用于路径规划/决策

场景：室内(房间/车库/仓库等)  / 室外

按照感知模式 

outside-in： 接收的外界信号包含位置相关信息 / GNSS UWB WIFI  Lidar-Hdmap Camera-marker(室外街景门牌室内maker) 磁条加marker(规划都做了) 扫地机本身贴marker加顶部相机  /   (适合测量方程) 

inside-out： 通过自身观察和感受获取位置信息 / IMU magnetometer  SLAM VR中的墙面贴marker建图  /  (适合状态方程) 

按照计算方法 

基于信号的定位： GNSS UWB  WIFI / 几何关系 

环境特征匹配： Lidar-HDmap Camera-marker Radar  / 需要建图，观测特征与数据库的特征匹配得到当前位姿态 

航迹推算： IMU odometry(轮子里程计/视觉里程计SLAM)  / 根据之前的位姿推断当前的位姿

定位系统的融合

原理：不同源传感器信息通过滤波器(KF EKF UKF PF等)进行融合 

典型融合方式： 

IMU+GNSS：高频率累计误差+低频率高精度                                 无人机/无人车/导弹/室外 

GNSS +  Lidar-HDmap/Camera-marker：适合开放环境+适合密集特征环境   无人车/室内外 

VIO(SLAM+IMU+GNSS)                                                                       无人机/无人车/室内外 

IMU+magnetometer  矫正角度(例如:互补滤波/MadgwickAHRS)           无人机/无人车/室内外

GNSS(GPS+BDS+GLONASS+GALILEO)

基于信号的定位： 车载/手机GPS接收机接收卫星发射的无线电信号 

问题1：无线电信号受到大气干扰无法准确计算GPS接收机到卫星的距离

解决：差分GPS的技术（RTK）。 在地面上建基站（Base Station）它有精确位置信息。

问题2：遮挡/反射问题

遮挡：GPS接收机在高楼周围，容易失去某一方向所有卫星信号，仅有特定方向的卫星信号使得定位精度降低 

反射：有些信号被大楼楼体反射之后才被接收到，称作多路径信号（Multi-Path Signal），多路径信号计算得到的距离会明显大于实际距离（因为时间偏大）。

解决： 仅靠GPS一种传感器很难在复杂场景中实现精确定位，必须引入其它互补传感器进行融合定位  

问题3：定位频率低 GPS定位的频率一般只有10Hz(100ms定位一次)

解决：引入其它传感器(例如IMU)提高定位频率。进而提高控制频率。

GNSS小结：

优点： 全球/全天候/全时段/高精度 

缺点： RTK差分技术的基站布设成本高，依赖可视卫星数量，易受电磁环境干扰，定位频率不够高，遮挡/反射(多径效应)

IMU

主要由陀螺仪和加速度计组成。消费级IMU(精度/价格低) ->光纤IMU(精度价格高)

加速度计能够测量出载体相对于惯性空间所受的力（进而得到加速度） 

陀螺仪能够测量出载体沿陀螺仪轴向的旋转角速度 

IMU是惯性导航系统的基础，直接决定了惯性导航系统所能够达到的精度

IMU的特点

优点：输出频率高，短时精度高，六自由度，可工作于任何复杂环境 

缺点： 误差随时间累积

Lidar-HDmap

预先制作定位地图3D/2D，车上的实时点云和地图进行匹配计算激光雷达的位置和姿态  

基于3D点云匹配， Scan Matching完成点云配准，常用的方法有两个：

ICP：最常见的是迭代最近点法（Iterative Closest Point ，ICP），该方法基于当前扫描和目标扫描的距离度量来完成点云配准 

NDT：正态分布变换（Normal Distributions Transform，NDT）也是进行点云配准的常用方法，它基于点云特征直方图来实现配准

激光定位特点： 

优点： 鲁棒性比较好/ 无GPS可工作 / 能实现10厘米以内的定位精度 

缺点： 需要预先制作地图并定期更新地图(环境会发生变化)，雨雪天气情况下Lidar被折射导致收到的点云数据变少，开放路段点云少仍需要GNSS（对于场景单一路段例如高速公路 GNSS+Lidar-HDmap成本过高）

Camera-marker 

街景定位（Sacha Arnoud, Director of Engineering, Waymo）：Waymo利用谷歌强大的街景数据标注出路牌号码,商店标志,红路灯等静态数据，来确定自身车辆所在的位置 

原理： 预先标注出路牌号码,商店牌匾,红绿灯等静态marker的全球坐标系。车载相机检测出maker之后通过透视原理或者相对距离推算出车辆自身的全球坐标 

优点： 简单直接：街景地图标记好之后，车辆本身只需要摄像机就可以实现精准的定位和导航 

缺点： 巨大的数据量和人工标注，摄像机采图问题：强光/弱光/逆光都会影响检查效果，基于图像计算深度图存在误差（越远视察越小 误差越大 z=fb/d），开放场景下(如高速公路) maker少甚至没有marker

SLAM:Simultaneous Localization and Mapping  同时定位与建图定位 ：

室内、室外  (我在哪？) 

建图：稀疏、半稠密、稠密 (环境如何？) 

准确的定位需要精确的地图,精确的地图来自准确的定位

机器人定位(例如扫地机)/手持设备定位(例如AR)/自动驾驶定位

类型：激光SLAM / 视觉SLAM(Monocular/Stereo/RGB-D) / VIO(IMU+SLAM)

视觉SLAM框架

前端：Visual Odometry  视觉里程计用于估计邻近时刻的相机运动(类似IMU航迹推算) 

后端：Optimization（滤波器法/图优化法）使用局部约束关系估计最优轨迹与地图 

回环检测：Loop Closing 检测是否形成了一个回环进而矫正整圈轨迹 

建图：Mapping  度量地图、拓扑地图、稀疏地图、稠密地图

定位融合技术

原理：不同源传感器信息通过滤波器(KF EKF UKF PF等)进行融合 优势互补

典型融合方式：

IMU+GNSS：高频率累计误差+低频率高精度                                  无人机/无人车/导弹/室外 

GNSS +  Lidar-HDmap/Camera-marker：适合开放环境+适合密集特征环境       无人车/室内外 

VIO(SLAM+IMU+GNSS)                                                                     无人机/无人车/室内外 

IMU+magnetometer  矫正角度(例如:互补滤波/MadgwickAHRS)           无人机/无人车/室内外

直方图滤波

本质就是先假设在各点的概率相同，在通过移动和观察逐步更新概率分布，最后用作定位。直方图滤波在无人驾驶中用到的比较多。

Apollo定位方案

二维直方图滤波器:-

中心放在预测姿态(x0, y0)，滤波器一般选择21*21=441个位置进行搜索 - predict(move):     - update(sense): SSD衡量观测与地图点的匹配程度