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

无人驾驶中定位的作用：

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

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

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

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

按照感知模式

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)           无人机/无人车/室内外