antd tree搜索并定位_自动驾驶定位思考

最近在做高精地图方面的工作，有一些疑问，这里把一些想法记录下来，希望能够抛砖引玉，对大家有所帮助。

高精地图的作用：一方面为我们提供地理位置信息，另一方面也为我们提供道路的辅助信息（红绿灯，路口，道路曲率，坡度，路况等）。

自动驾驶定位

我们先看一下无人车如何定位。众所周知，自动驾驶的第一步就是定位，知道自己在哪里，才能进行路径规划，控制汽车行驶。当前汽车主要用GPS导航，由于GPS不是很精准，95%的情况都可以在2米以内，在人驾驶汽车的情况下，只需要知道大概位置就可以了，人可以根据周围的环境来判断自己的位置，而自动驾驶目前还没那么智能，所以对位置的要求要更高，如果误差太大，可能出现定位错误的情况，比如你不在路口，定位到路口了，这个时候车可能会做出错误的预判。

其实我认为自动驾驶后面的方向也是，只知道大概位置，然后靠感知去控制局部的驾驶，这个时候感知接管了驾驶。

GPS定位也有一些问题，首先精度不够高，然后人们又利用RTK技术，即建立一个基站，基站的位置是已知的精确坐标（可以事先测量），然后通过基站和终端同时发送GPS定位，通过基站查询的位置和基站的绝对位置得出一个偏差，然后终端通过偏差来校正自己的位置。在信号比较好的情况下，得到的精度可以达到厘米级别，这样的精度已经达到要求了。另外的问题是GPS的频率是10hz，所以位置信息0.1s才更新一次，这0.1s里的位置发生了变化，可以通过IMU和里程计来计算，即通过计算汽车的姿态，初速度，加速度来计算出汽车的实时位置，通过上述的技术解决了自动驾驶汽车的定位问题。

目前我们需要的高精度定位，用上面介绍的技术就可以实现了，为什么我们还需要点云地图呢？而且如果要绘制点云地图，我们还是会依赖位置信息来绘制，所以，单纯的绘制点云没有太大意义，假设我们的点云绘制过程如下，开车转圈，先采集一圈的点云数据，然后逐圈迭代，得出一个点云的地图，问题是点云的世界坐标基准又从哪里来呢，又是从GPS来的。所以点云的位置信息，实际也是依赖GPS，因此在有GPS的情况，都可以采用GPS去定位，除非遇到GPS失效的情况下，我们才需要用到点云去定位。

那么哪些情况下GPS会失效呢，可以想到的情况是：一是天气，在下大雨有云层这种情况下连自动驾驶的感知都会失效；然后是地形，隧道，高架桥这种场景会出现小号弱或者多径效应。所以在GPS失效的情况下，用激光雷达的点云来辅助定位。

如果所有位置都丢了，即GPS的位置也不准，点云也没有找到具体的位置，百度给出介绍是车将会根据车道线驾驶，所以我觉得后面车应该会根据感知，具备一定的自动预判位置的能力，而不是单纯靠位置信息。

定位总结

定位模块首先会根据GPS和IMU获取位置信息，如果上述位置信息不准的情况下，会采用点云地图做匹配的情况下，获取位置信息，如果上述2种方法都失效的情况下，会根据车道线驾驶，如果连续找不到位置可能会被接管或者停车？

另外我的一个设想是，后面车会根据路牌，商店，交通标志等判断自己当前的位置，这一块google已经有做相关的研究了。这样我们只需要知道定位模块会依赖一系列的融合技术告诉你车的位置，并且这个位置信息在100000小时只有1次失误，那么是不是就可以认为定位模块达到了车规级，并且我们就相信这个定位模块已经足够好了？

点云地图的制作

点云地图，我认为还存在2个问题，一是数据本身的问题，点云的数据太大，如果采用点云定位，存储的地图容量非常大，而且需要做计算；点云的数据需要实时更新，维护起来成本太高，也很难做到实时；二是点云的定位本质还是搜索，如果在高速和隧道上一段没有特点的道路，用点云定位这种方式效果就不会太好。在我看来如果重复度高的数据，与其保存，还不如直接舍弃，没什么实质性的帮助。

基于上面的想法，其实我倒是觉得点云在室内定位的时候很有帮助，因为没有GPS信号，比起把传感器放到室内机器人上，不如把雷达放到屋顶，这样可以应用到多个机器人。如果固态的激光雷达只需要几千美金的情况下，肯定会在室内占据更大的市场。比一些WIFI信号靠谱多了。
点云定位受限于最远扫描距离200米，以及点云特征，实际上有些场景，比如高速上并不适合，如果有指示牌告诉你下一个出口就是200米，高速上的某个地标，或者救援坐标，隧道里的坐标等，都可以更加详细的告诉你位置，而这些需要通过摄像头来实现比较方便。
感知由于计算的频率更高，是不是可以把处理好的结果直接丢给定位，然后定位去处理这些视觉信息，定位的优先级低于感知？

建图

下面主要讲下点云地图的制作，实际上就是SLAM的过程，实际上如果GPS信息良好的路段，我觉得不需要制作点云地图，只有信号差的路段，我们才需要制作点云地图，而制作地图又不能依赖GPS信号，那么我们需要一个基准，目前能想到的就是依赖IMU去计算，或者单纯的依赖SLAM算法去配准，而SLAM算法都存在处理不好闭环问题的情况，因此我觉得用IMU去校准位置比较好一点。

1. 首先根据固定的线路绘制地图，由于一次采集的数据不可能太完整，需要多次采集。这里多次采集一定是必须的，比如左右行驶视角不一样，十字路口不同的方向经过看到的是不同的3D视角，如果只跑一遍，得不出完整的3维地图。

2.反复迭代上述过程，进行建图

最后我们得到大量的点云数据，并且得到一些不太精准的GPS位置信息，需要对数据做处理，常见的算法就是对点云数据做校准合并，即使同一位置每次采集都会有一定偏差，所以合并的时候需要校准。

PCL库已经提供了相应的算法

1. 迭代最近点 Iterative Closest Point
2. NDT mapping

根据上面的信息，迭代几轮，就可以制作出地图了。具体的文章可以参考

One way Google’s cars localize themselves - Mapping Ignorance

查找位置

最先想到的办法就是把这个问题转换为字符串匹配搜索，即在大图中找到子图，还不是简单的图片搜索，和子串搜索的问题比较类型。可以参考

查找的基本思路就是做匹配，和图像匹配思路差不多，最近这2年对大数据的场景都不要求精确匹配了，而是查找近似最优，目前用到的算法是flann算法，得到的信息和点云地图做匹配，就可以得到车辆的位置信息。

这里对点云地图的数据结构也有要求，可以使用kd-tree来处理，当然数据库的常用操作，增加，删除，插入，kd-tree的维护都需要考虑进去，这就要求地图肯定是离散的设计，不会设计成为一个整体？

下面是一些思考：

百度是采用了深度信息和高度信息的点云地图，地图的栅格大小为3.2*3.1km的大小，如果是已经知道大概位置，只需要做最近邻匹配就可以了，这个问题转化为配准问题，第一帧是你之前的位置，第二帧是你现在的位置，这2个位置之间有重合，然后你通过变换来确认当前的速度以及姿态，如果按照2帧需要50%的重叠来说，对车的速度位30KM/小时来算，0.1s车只跑了0.83米，可以说大部分时候前后2帧都是重叠的，可以通过校准的算法来计算当前车的位置。

这里是采用雷达扫描出来的2帧做对比，还是用当前位置的帧和地图中的做比较？如果是雷达扫出来的2帧，那么不需要更新地图了，因为地图中的数据可能没更新，比如下雪了。如果是采用当前位置和地图中做比较，则显得精确一点，没有累计误差？

如果不知道大概位置，需要做全局搜索，我们可以计算一下，激光点云的最大扫描距离是200米，我们按照100米作为有效距离计算，每次移动1米逐行在3.2*3.1km的地图中查找，那么需要查找(3200-100)*(3100-100)=930万次，如果顺序查找即需要最坏930万次，如果利用点云地图的结构化信息，按照特征建立kd-tree，那么查找的时间会大大减少，假如按照2分法分割地图的点，那么只需要查找25次查找就可以，当然实际情况会需要更多。

而且上述的查找问题，实际上可以转换为栅格地图的分类问题，分类的越均匀，那么查找的时间越短，那么我们是不是可以利用机器学习来对地图查找特征并且分类呢？

然后是数据大小，目前激光雷达的频率是1HZ，即0.1S产生一次数据，如果一帧的数据是30W个点，那么可以大概估算下30min的数据量是多大？

特征提取

点云抽样和特征提取相对可以加快数据的搜索，关于特征的提取有什么相关的好算法？

抽样和特征提取的好处是加快查找速度，另外的好处是减少数据的大小。

回顾

大部分情况下都依赖GPS，只有特殊场景隧道，高架，高楼旁边才依赖点云去查找位置。

点云制作过程，是反复迭代并且用最近点（Iterative Closest Point）算法合并点云，然后用flann算法去查找位置

数据库设计要保证增加，删除，插入和维护kd_tree的速度足够快，更好的提供云服务

抽样和特征提取可以加快查找速度，减少点云地图的数据大小

daohu527/Dig-into-Apollo​github.com

参考资料

地图制作

One way Google’s cars localize themselves - Mapping Ignorance

点云合并

FLANN - FLANN browse

查找

Point Cloud Library (PCL)

查找子窜

https://zh.wikipedia.org/wiki/%E5%AD%97%E4%B8%B2%E6%90%9C%E5%B0%8B%E6%BC%94%E7%AE%97%E6%B3%95

论文

Finding a subimage inside a Numpy image

Efficient Near-duplicate Detection and Sub-image Retrieval

图像查找原理

Template Matching By Normalized Cross Correlation