前言：这节主要介绍无人车定位系统里面使用到的定位算法。

概要：
1、定位技术简介
2、单个定位技术
3、定位融合技术
4、Apollo定位方案

1、定位技术简介

1.1 定位作用：

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

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

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

1.2 定位技术分类

1）场景：室内（房间/车库/厂库等）/室外

2）按定位模式：
----outside-in（从外向内）:接收的外界信号包含位置相关信息/（这种模式适合测量方程）
例子：GNSS、UWB、WIFI、Lidar-Hdmap、Camera-marker(室外街景门牌内maker)、磁条加marker（规划都做了）、扫地机本身贴marker加顶部相机

----inside-out（从内到外）：通过自身观察和感受获取位置信息/（这种模式适合状态方程）
例子：IMU 、magnetometer【磁力计，原理是磁通量来计算角度】、 SLAM【主动观测特征点】、VR中的墙面贴marker建图

【GNSS：全球导航卫星信息系统，如GPS；UWB：电信号】

3）按照计算方法：
----基于信号的定位：GNSS UWB WIFI/几何关系
----环境特征匹配：Lidar-HDmap【雷达扫描的，和高精地图比对，从而定位】、Camera-marker Radar/【这种方式】需要建图，观测特征与数据库的特征匹配得到当前姿态

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

1.3 定位系统的融合

1）原理：不同源传感器信息通过滤波器（KF EKF UKF PF等）【KF表示卡尔曼滤波；PF表示粒子滤波】进行融合

2）典型的融合方式：
----IMU+GNSS:高频率累计误差+低频率高精度 【IMU大概100hz;GNSS大概10hz】
应用：无人车/无人机/导弹/室外

----GNSS+Lidar+HDmap/Camera-marker【这里的marker可以是门牌号这些】:适合开发环境+适合密集特征环境
应用：无人车/室内外

----VIO(视觉SLAM+IMU+GNSS)【V表示视觉惯性里程计；I表示IMU;】
应用：无人车/无人机/室内外

----IMU+magnetometer 矫正角度（例如：互补滤波/MadgwickAHRS）
应用：无人车/无人机/室内外

2、单个定位技术

2.1GNSS

1)GPS （global position system）
GNSS(GPS+BDS+GLONASS+GALILEO)
【BDS：中国的北斗；GLONASS：俄罗斯卫星定位系统；GALILEO：欧洲的伽利略系统】

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

----三角定位法（Triangulation）【见图1】
--------GPS 接收机计算到达卫星的距离：TOF测距原理：距离R=无线电信号从卫星到接收机的传播时间T*光速C

---------每个卫星半径为R的球体表面是GPS接收机可能的位置
--------3个卫星产生的球面的交点就是GPS接收机的位置

3）问题1：无线电信号受到大气干扰无法准确计算GPS接收机到卫星的距离：无线电信号的传播时间受受传播介质的影响，例如云层的稀薄情况，天气情况

----解决：差分GPS技术（RTK）。在地面上建基站（Base Station）它有精确位置信息。也接收卫星信号，根据信号传播时间计算的位置与基站的精确位置比较，可以推算出此时的信号传播误差。当车载GPS接收机与基站的GPS接收机距离较近时（<30km）,可以认为两者的GPS信号通过的是同一片大气区域具有相似的误差。就可以用基站推算出误差去修正GPS的位置。
【内容参考：https://zhuanlan.zhihu.com/p/32638633】

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

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

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

5）问题3：定位频率低
----GPS定位的频率一般只有10HZ（100ms定位一次）；72km/h(20m/s)运动的汽车一个定位周期（100ms）内已经移动了20m/s*01s=2m。弯道情况下每隔2m才控制一次运动方向可能导致车辆沿弯道一会左偏，一会右偏。

----解决：引入其他传感器（例如IMU）提供定位频率，进而提高控制频率

6）GNSS小结：
优点：全球、全天候、全时段、高精度

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

2.2 IMU

1）捷联惯性导航（SINS：strapdown inertial navigation system）技术：

----惯导系统是一种不依赖任何外部信息、也不向外部辐射能量的自主式导航系统，隐蔽性好，可在空中、地面、水下等各种复杂环境下工作

----主要分为平台式惯导系统和捷联式惯导系统两大类。捷联惯导系统是在平台式惯导系统基础上发展而来的，它是一种无框架系统（没有物理平台也没有数字平台,通过计算进行姿态变换，结构简单、体积小、重量轻、成本低、维护简单、可靠性高）

----捷联（Strapdown）式“捆绑”的意思。所谓捷联惯性系统是将惯性敏感元件（陀螺和加速度计）直接“捆绑”在运载体的机体上，从而完成制导和导航任务的系统

----陀螺和加速度计分别用来测量运载体的角运动信息（角速度）和线运动信息（加速度）

----经过解算得到载体的速度、位置、姿态

2）惯性测量单元（IMU）

----主要有陀螺仪和加速度计组成。消费级IMU（精度价格低）–>光纤IMU（精度价格高）
----加速度计能够测量出载体相对惯性空间所受的力（进而得到加速度）

----陀螺仪能够测量出载体沿陀螺仪轴向的旋转角速度
----IMU是惯性导航系统的基础，直接决定了惯性导航系统所能够达到的精度

3）IMU小结：
优点：
----输出频率高
----短时精度高
----六自由度（角速度，加速度各3个方向）
----可工作在任何复杂环境

缺点：误差随时间累积

2.3 Lidar/Camera

1)环境特征匹配：Lidar-HDmap/Camera-marker

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

----如何匹配？
--------基于3D点云匹配：Scan Matching完成点云匹配
-------------ICP：最常见的迭代最近点法（Iterative Closest Point,ICP）,该方法基于当前扫描和目标扫描的距离度量来完成点云配准
-------------NDT：正态分布变换（Normal Distributions Transform,NDT）也是进行点云配准的常用方法，它基于点云特征直方图来实现配准

--------基于2D概率地图的直方图滤波匹配定位

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

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

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

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

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

—缺点：
------巨大的数据量和人工标注
------摄像机采图问题：强光/弱光/逆光都会影响检查效果，进而导致推算的位置不准

------基于图像计算深度图存在误差（越远视差（察）越小，误差越大z=fb/d fb焦距;d:视差）
------开发场景下（如高速公路）marker少甚至没有marker

2.4 SLAM

1）SLAM：Simultaneous Localization and Mapping 同时定位与建图
----定位：室内、室外（解决 我在哪？ 的问题）
----建图：稀疏、半稠密、稠密（环境如何？）
----准确的定位需要精确的地图，精确的地图来自精确的定位

2）类型：激光SLAM/视觉SLAM（Moncular/Stereo/RGB-D）/VIO(IMU+SLAM)

3)主要应用方向：
–机器人定位（例如扫地机器人）/手持设备定位（例如AR）/自动导航定位

4）视觉SLAM框架

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

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

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

----建图：Mapping 度量地图、拓扑地图、稀疏地图、稠密地图
【这个建图的关键公式】

【入门到深入教材使用；老师建议看完左1，接着看ORB开源方案代码，接着VIO开源方案代码】

纯视觉定位例子：https://www.youtube.com/watch?v=j2ZNuBCr4SA

3、定位融合技术

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

2）典型融合方式：
----IMU+GNSS:高频率累计误差+低频率高精度
---------应用场景：无人车、无人机、导弹、室外

----GNSS+Lidar-HDmap/Camera-marker:适合开发环境+适合密集特征环境外
---------应用场景：无人车、室内外

----VIO（SLAM+IMU+GNSS）
---------应用场景：无人车、无人机、室内外
----IMU+magnetometer 矫正角度【磁力计矫正IMU的角度】（例如：互补滤波/MadgwickAHRS）
---------应用场景：无人机、无人机、室内外

3.1 Histogram Filter（直方图滤波）
1）

左侧是小车，右侧5格子是小车的运动范围，循环往复运动
参考：Udacity <> Lesson1

初始化位置x1:刚开始自己位置完全不知，均匀分布表示在任何位置的概率都是均等的
向右一步移动到x2:观察一次发现自己在红色格子里。可以根据观察更新一下概率分布（红色0.6绿色0.2并概率化）【0.6是人为定的】
【绘图简单理解，一开始小车并不知道自己所在位置，每个格子可行性相等；移动一步后，并观察到自己所在区域是红色格子，那么就将红色部分按照权值0.6相乘，绿色*0.4，获取新的格子概率，得到上面绘图第二行数字，不过凸显了红色和绿色的；但是，此时会发现概率之和不为1了，此时将除以当前概率总和，得到第三行数字；那么，绘制图案中的概率P表示的是在观测Z的条件下的xi发生的概率】
下面的代码就是实现绘制图的

2）确定性移动：

----再次向右精确的走一步（假设走到最右端回到最开始的位置继续走）
----设计一个move函数:输入概率分布p和移动步数U（-1表示向左移1步；2表示向右移动2步;正负表示右左，数字表示步数）输出移动之后的概率分布p

【绘图表示的是向右移动一步】

【输入移动前每格的概率分布，以及步数，返回移动后的概率分布】
P[(i-U)%len(p）]:（i-U）表示当前格概率是没有移动前的概率值，求余运算是由于循环往复原因。

3）不确定性的移动
----实际上不可能每次移动都保证很精确，U=2移动时的不确定性（虽然说是移动了2格，但是真正移动多少格属于不确定性的）如图1表达
----假设p=[0 1 0 0 0]输出什么分布？
----假设p=[0 0.5 0 0.5 0]输出什么分布？
----假设p=[0.2 0.2 0.2 0.2 0.2]输出什么分布？

【线的数字表示移动到该格的权值，这里表示达到格子的可能性】


【下行的概率数值，都是通过移动2格0.8，移动1格或3格0.1，计算所得，对于有多个移动，则叠加】


【这里面pExact pOvershoot pUndershoot表示正常移动，走多，走少时所对应的权重0.8，0.1，0.1；最后返回的是，各个格子概率分布;老师说，这类似滑窗时的卷积计算】

4）例子：移动带来不确定性
----p=[0,1,0,0,0]移动一次p=[0,0,0.1,0.8,0.1,0]
----如果连续移动两次p=[0.001,0.001,0.16,0.66,0.16]
----连续移动1000次，p=[0.2,0.2,0.2,0.2,0.2]又回到了完全不确定了，再怎么移动都无法改变概率分布了，必须进行观测增加信息

5）每次移动会丢失信息，每次观察会增加信息


【sense表示观察；move表示移动；两者配合才获取比较准确的值】

4、Apollo定位方案

参考：
论文：https://arxiv.org/pdf/1711.05805.pdf《Robust and Precise Vehicle Localization based on Multi-sensor Fusion in Diverse City Scenes》
作者演讲：https://mp.weixin.qq.com/s/89xbHeS8RtS1L_fBe1VMDw


【PVA方差；VCV协方差；GNSS、LIDAR定位都可以给出观测结果，指向的是观测方程，而且两者可以接收SINS预测的PVA；SINS里面是运动方程】

2）Apollo定位方案

Attitude error指的是姿态误差；accelerometers bias 加速度偏置；gyroscopes bias角速度偏置

6-DoF:position有三个，xyz；attitude有三个，ryp.

3)点云定位模块




二维直方图滤波器：
----中心放在预测姿态（x0,y0）,滤波器一般选择21*21=441个位置进行搜索【每个格子有个置信度，会选择置信度最高的作为预测位置，这里的置信度可以理解为匹配效果】【如右图的红色区域置信度最高，就是激光雷达数据得到的位置】

----predict(move)：【使用高斯分布，预测移动到达位置】

----update(sense):SSD衡量观测与地图点的匹配程度【SSD显示的误差如果比较大的话，表示匹配不好，则将该区域的权重选小一些】
【SSD-HF表示直方图滤波，SSD表示误差的平方和；表示的是将误差平方和来判断匹配的效果；根据匹配结果来打分，如残差比较小，则打分比较高】

老师分享学习SLAM经历（经验）

1）从CV转SLAM，网上搜索知道高翔讲SLAM比较好，也有他一本书叫SLAM十四讲；

2）过年时，开始自学这本书，抱着能理解多少算多少心态，把这本书看了一遍；

3）需要把框架体系整理起来，也就是把SLAM基本概念要有才行；

4）看完十四讲，看得似懂非懂，后面直接看ORB代码

5）ORB代码，从前端开始看，然后看后端。因为工作原因，所以看得比较断续。

6）当把ORB代码看差不多时候，感觉通了，对SLAM有了很好得认识

7）ORB代码就是讲述框架体系的东西

8）必须看代码才能知道是（框架体系内容或者概念）怎么操作的

9）十四讲的代码量比较小，跑一跑理解基本概念是可以的；系统整体性考虑，可能看一些开源的，比如ORB，比较好一些

10）看完十四讲后（也搞懂了ORB代码后），回头再看书，会发现，很流畅了

11）建议看代码时，不应该抠比较细节东西，细节东西，如某矩阵怎么求的；应该先理清整体框架，比如应该知道怎么运行该程序，前端怎么跑的，怎么选取关键帧的，前端怎么跟后端对接，后端是怎么拿到做公示图做优化

12）有了代码的整体框架，回头查询细节东西，通过blog啥的，会很容易理解消化该知识点了。

13）有了上面的经验，就开始看VIO方面的知识。

14）看VIO，直接找一个开源的项目来看。其中VINS资源比较多

15）看VIO，一定只挑一个方案来看，细看，直到看懂（比如只选择VINS）；看懂一个，其他就是依此类推的

16）卡尔曼kalman滤波的概念很关键；ROstate的卡尔曼滤波掌握了，就有了一个全局的概念；有了全局概念，再看MSCKFVIO，这个代码量比较小，比较好理解。老师说，接下来就是研究VINS的代码

17）https://gitee.com/paopaoslam/ORB-SLAM2 ORB-slam中文注释版学习

18）其他关于ORB代码学习，可以再GitHub上面搜一些初学者的代码注释，一般会写的很详细的

19）无人驾驶的方向很多，授课的面很广，需要有针对性选择来深入学习，提供应聘成功率

20）人的精力有限，需要选择一个方向来专攻。举个例子，单纯高精地图就是一个组的人来负责的；而视觉里面，单纯点云方面就有好几个岗位。

21）阿波罗的rtk，用的是原生的GPS数据，自己计算的，从而方便查看哪个方面产生偏差

22）GPS数据，有原生类型，也有经过处理好的

23）激光雷达是可以做SLAM的，将激光作为特征来使用；Apollo是使用高精地图加激光匹配方式实现的，跟单纯激光SLAM有点不一样的。

个人小结：这节课介绍了无人车的定位算法方面知识。首先对定位技术做了简单介绍，然后
介绍常见的单定位技术：GNSS、IMU、lidar-HDmap/Camera-marker，SLAM，也点出了其中有些不足。接着，引入定位融合技术的知识。介绍了几种融合定位技术，最后介绍Apollo定位方案，作为融合定位技术的例子。

#####################
感恩授课老师的付出
图片版权归原作者所有
不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈