最近在做高精地圖方面的工作，有一些疑問，這里把一些想法記錄下來，希望能夠拋磚引玉，對大家有所幫助。

高精地圖的作用：一方面為我們提供地理位置信息，另一方面也為我們提供道路的輔助信息（紅綠燈，路口，道路曲率，坡度，路況等）。

自動駕駛定位

我們先看一下無人車如何定位。眾所周知，自動駕駛的第一步就是定位，知道自己在哪里，才能進行路徑規劃，控制汽車行駛。當前汽車主要用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庫已經提供了相應的算法

迭代最近點 Iterative Closest Point

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

總結

以上是生活随笔為你收集整理的antd tree搜索并定位_自动驾驶定位思考的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。