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

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

自動(dòng)駕駛定位

我們先看一下無(wú)人車(chē)如何定位。眾所周知，自動(dòng)駕駛的第一步就是定位，知道自己在哪里，才能進(jìn)行路徑規(guī)劃，控制汽車(chē)行駛。當(dāng)前汽車(chē)主要用GPS導(dǎo)航，由于GPS不是很精準(zhǔn)，95%的情況都可以在2米以內(nèi)，在人駕駛汽車(chē)的情況下，只需要知道大概位置就可以了，人可以根據(jù)周?chē)沫h(huán)境來(lái)判斷自己的位置，而自動(dòng)駕駛目前還沒(méi)那么智能，所以對(duì)位置的要求要更高，如果誤差太大，可能出現(xiàn)定位錯(cuò)誤的情況，比如你不在路口，定位到路口了，這個(gè)時(shí)候車(chē)可能會(huì)做出錯(cuò)誤的預(yù)判。

其實(shí)我認(rèn)為自動(dòng)駕駛后面的方向也是，只知道大概位置，然后靠感知去控制局部的駕駛，這個(gè)時(shí)候感知接管了駕駛。

GPS定位也有一些問(wèn)題，首先精度不夠高，然后人們又利用RTK技術(shù)，即建立一個(gè)基站，基站的位置是已知的精確坐標(biāo)（可以事先測(cè)量），然后通過(guò)基站和終端同時(shí)發(fā)送GPS定位，通過(guò)基站查詢的位置和基站的絕對(duì)位置得出一個(gè)偏差，然后終端通過(guò)偏差來(lái)校正自己的位置。在信號(hào)比較好的情況下，得到的精度可以達(dá)到厘米級(jí)別，這樣的精度已經(jīng)達(dá)到要求了。另外的問(wèn)題是GPS的頻率是10hz，所以位置信息0.1s才更新一次，這0.1s里的位置發(fā)生了變化，可以通過(guò)IMU和里程計(jì)來(lái)計(jì)算，即通過(guò)計(jì)算汽車(chē)的姿態(tài)，初速度，加速度來(lái)計(jì)算出汽車(chē)的實(shí)時(shí)位置，通過(guò)上述的技術(shù)解決了自動(dòng)駕駛汽車(chē)的定位問(wèn)題。

目前我們需要的高精度定位，用上面介紹的技術(shù)就可以實(shí)現(xiàn)了，為什么我們還需要點(diǎn)云地圖呢？而且如果要繪制點(diǎn)云地圖，我們還是會(huì)依賴位置信息來(lái)繪制，所以，單純的繪制點(diǎn)云沒(méi)有太大意義，假設(shè)我們的點(diǎn)云繪制過(guò)程如下，開(kāi)車(chē)轉(zhuǎn)圈，先采集一圈的點(diǎn)云數(shù)據(jù)，然后逐圈迭代，得出一個(gè)點(diǎn)云的地圖，問(wèn)題是點(diǎn)云的世界坐標(biāo)基準(zhǔn)又從哪里來(lái)呢，又是從GPS來(lái)的。所以點(diǎn)云的位置信息，實(shí)際也是依賴GPS，因此在有GPS的情況，都可以采用GPS去定位，除非遇到GPS失效的情況下，我們才需要用到點(diǎn)云去定位。

那么哪些情況下GPS會(huì)失效呢，可以想到的情況是：一是天氣，在下大雨有云層這種情況下連自動(dòng)駕駛的感知都會(huì)失效；然后是地形，隧道，高架橋這種場(chǎng)景會(huì)出現(xiàn)小號(hào)弱或者多徑效應(yīng)。所以在GPS失效的情況下，用激光雷達(dá)的點(diǎn)云來(lái)輔助定位。

如果所有位置都丟了，即GPS的位置也不準(zhǔn)，點(diǎn)云也沒(méi)有找到具體的位置，百度給出介紹是車(chē)將會(huì)根據(jù)車(chē)道線駕駛，所以我覺(jué)得后面車(chē)應(yīng)該會(huì)根據(jù)感知，具備一定的自動(dòng)預(yù)判位置的能力，而不是單純靠位置信息。

定位總結(jié)

定位模塊首先會(huì)根據(jù)GPS和IMU獲取位置信息，如果上述位置信息不準(zhǔn)的情況下，會(huì)采用點(diǎn)云地圖做匹配的情況下，獲取位置信息，如果上述2種方法都失效的情況下，會(huì)根據(jù)車(chē)道線駕駛，如果連續(xù)找不到位置可能會(huì)被接管或者停車(chē)？

另外我的一個(gè)設(shè)想是，后面車(chē)會(huì)根據(jù)路牌，商店，交通標(biāo)志等判斷自己當(dāng)前的位置，這一塊google已經(jīng)有做相關(guān)的研究了。這樣我們只需要知道定位模塊會(huì)依賴一系列的融合技術(shù)告訴你車(chē)的位置，并且這個(gè)位置信息在100000小時(shí)只有1次失誤，那么是不是就可以認(rèn)為定位模塊達(dá)到了車(chē)規(guī)級(jí)，并且我們就相信這個(gè)定位模塊已經(jīng)足夠好了？

點(diǎn)云地圖的制作

點(diǎn)云地圖，我認(rèn)為還存在2個(gè)問(wèn)題，一是數(shù)據(jù)本身的問(wèn)題，點(diǎn)云的數(shù)據(jù)太大，如果采用點(diǎn)云定位，存儲(chǔ)的地圖容量非常大，而且需要做計(jì)算；點(diǎn)云的數(shù)據(jù)需要實(shí)時(shí)更新，維護(hù)起來(lái)成本太高，也很難做到實(shí)時(shí)；二是點(diǎn)云的定位本質(zhì)還是搜索，如果在高速和隧道上一段沒(méi)有特點(diǎn)的道路，用點(diǎn)云定位這種方式效果就不會(huì)太好。在我看來(lái)如果重復(fù)度高的數(shù)據(jù)，與其保存，還不如直接舍棄，沒(méi)什么實(shí)質(zhì)性的幫助。

基于上面的想法，其實(shí)我倒是覺(jué)得點(diǎn)云在室內(nèi)定位的時(shí)候很有幫助，因?yàn)闆](méi)有GPS信號(hào)，比起把傳感器放到室內(nèi)機(jī)器人上，不如把雷達(dá)放到屋頂，這樣可以應(yīng)用到多個(gè)機(jī)器人。如果固態(tài)的激光雷達(dá)只需要幾千美金的情況下，肯定會(huì)在室內(nèi)占據(jù)更大的市場(chǎng)。比一些WIFI信號(hào)靠譜多了。
點(diǎn)云定位受限于最遠(yuǎn)掃描距離200米，以及點(diǎn)云特征，實(shí)際上有些場(chǎng)景，比如高速上并不適合，如果有指示牌告訴你下一個(gè)出口就是200米，高速上的某個(gè)地標(biāo)，或者救援坐標(biāo)，隧道里的坐標(biāo)等，都可以更加詳細(xì)的告訴你位置，而這些需要通過(guò)攝像頭來(lái)實(shí)現(xiàn)比較方便。
感知由于計(jì)算的頻率更高，是不是可以把處理好的結(jié)果直接丟給定位，然后定位去處理這些視覺(jué)信息，定位的優(yōu)先級(jí)低于感知？

建圖

下面主要講下點(diǎn)云地圖的制作，實(shí)際上就是SLAM的過(guò)程，實(shí)際上如果GPS信息良好的路段，我覺(jué)得不需要制作點(diǎn)云地圖，只有信號(hào)差的路段，我們才需要制作點(diǎn)云地圖，而制作地圖又不能依賴GPS信號(hào)，那么我們需要一個(gè)基準(zhǔn)，目前能想到的就是依賴IMU去計(jì)算，或者單純的依賴SLAM算法去配準(zhǔn)，而SLAM算法都存在處理不好閉環(huán)問(wèn)題的情況，因此我覺(jué)得用IMU去校準(zhǔn)位置比較好一點(diǎn)。

1. 首先根據(jù)固定的線路繪制地圖，由于一次采集的數(shù)據(jù)不可能太完整，需要多次采集。這里多次采集一定是必須的，比如左右行駛視角不一樣，十字路口不同的方向經(jīng)過(guò)看到的是不同的3D視角，如果只跑一遍，得不出完整的3維地圖。

2.反復(fù)迭代上述過(guò)程，進(jìn)行建圖

最后我們得到大量的點(diǎn)云數(shù)據(jù)，并且得到一些不太精準(zhǔn)的GPS位置信息，需要對(duì)數(shù)據(jù)做處理，常見(jiàn)的算法就是對(duì)點(diǎn)云數(shù)據(jù)做校準(zhǔn)合并，即使同一位置每次采集都會(huì)有一定偏差，所以合并的時(shí)候需要校準(zhǔn)。

PCL庫(kù)已經(jīng)提供了相應(yīng)的算法

迭代最近點(diǎn) Iterative Closest Point

NDT mapping

根據(jù)上面的信息，迭代幾輪，就可以制作出地圖了。具體的文章可以參考

One way Google’s cars localize themselves - Mapping Ignorance

查找位置

最先想到的辦法就是把這個(gè)問(wèn)題轉(zhuǎn)換為字符串匹配搜索，即在大圖中找到子圖，還不是簡(jiǎn)單的圖片搜索，和子串搜索的問(wèn)題比較類型。可以參考

查找的基本思路就是做匹配，和圖像匹配思路差不多，最近這2年對(duì)大數(shù)據(jù)的場(chǎng)景都不要求精確匹配了，而是查找近似最優(yōu)，目前用到的算法是flann算法，得到的信息和點(diǎn)云地圖做匹配，就可以得到車(chē)輛的位置信息。

這里對(duì)點(diǎn)云地圖的數(shù)據(jù)結(jié)構(gòu)也有要求，可以使用kd-tree來(lái)處理，當(dāng)然數(shù)據(jù)庫(kù)的常用操作，增加，刪除，插入，kd-tree的維護(hù)都需要考慮進(jìn)去，這就要求地圖肯定是離散的設(shè)計(jì)，不會(huì)設(shè)計(jì)成為一個(gè)整體？

下面是一些思考：

百度是采用了深度信息和高度信息的點(diǎn)云地圖，地圖的柵格大小為3.2*3.1km的大小，如果是已經(jīng)知道大概位置，只需要做最近鄰匹配就可以了，這個(gè)問(wèn)題轉(zhuǎn)化為配準(zhǔn)問(wèn)題，第一幀是你之前的位置，第二幀是你現(xiàn)在的位置，這2個(gè)位置之間有重合，然后你通過(guò)變換來(lái)確認(rèn)當(dāng)前的速度以及姿態(tài)，如果按照2幀需要50%的重疊來(lái)說(shuō)，對(duì)車(chē)的速度位30KM/小時(shí)來(lái)算，0.1s車(chē)只跑了0.83米，可以說(shuō)大部分時(shí)候前后2幀都是重疊的，可以通過(guò)校準(zhǔn)的算法來(lái)計(jì)算當(dāng)前車(chē)的位置。

這里是采用雷達(dá)掃描出來(lái)的2幀做對(duì)比，還是用當(dāng)前位置的幀和地圖中的做比較？如果是雷達(dá)掃出來(lái)的2幀，那么不需要更新地圖了，因?yàn)榈貓D中的數(shù)據(jù)可能沒(méi)更新，比如下雪了。如果是采用當(dāng)前位置和地圖中做比較，則顯得精確一點(diǎn)，沒(méi)有累計(jì)誤差？

如果不知道大概位置，需要做全局搜索，我們可以計(jì)算一下，激光點(diǎn)云的最大掃描距離是200米，我們按照100米作為有效距離計(jì)算，每次移動(dòng)1米逐行在3.2*3.1km的地圖中查找，那么需要查找(3200-100)*(3100-100)=930萬(wàn)次，如果順序查找即需要最壞930萬(wàn)次，如果利用點(diǎn)云地圖的結(jié)構(gòu)化信息，按照特征建立kd-tree，那么查找的時(shí)間會(huì)大大減少，假如按照2分法分割地圖的點(diǎn)，那么只需要查找25次查找就可以，當(dāng)然實(shí)際情況會(huì)需要更多。

而且上述的查找問(wèn)題，實(shí)際上可以轉(zhuǎn)換為柵格地圖的分類問(wèn)題，分類的越均勻，那么查找的時(shí)間越短，那么我們是不是可以利用機(jī)器學(xué)習(xí)來(lái)對(duì)地圖查找特征并且分類呢？

然后是數(shù)據(jù)大小，目前激光雷達(dá)的頻率是1HZ，即0.1S產(chǎn)生一次數(shù)據(jù)，如果一幀的數(shù)據(jù)是30W個(gè)點(diǎn)，那么可以大概估算下30min的數(shù)據(jù)量是多大？

特征提取

點(diǎn)云抽樣和特征提取相對(duì)可以加快數(shù)據(jù)的搜索，關(guān)于特征的提取有什么相關(guān)的好算法？

抽樣和特征提取的好處是加快查找速度，另外的好處是減少數(shù)據(jù)的大小。

回顧

大部分情況下都依賴GPS，只有特殊場(chǎng)景隧道，高架，高樓旁邊才依賴點(diǎn)云去查找位置。

點(diǎn)云制作過(guò)程，是反復(fù)迭代并且用最近點(diǎn)（Iterative Closest Point）算法合并點(diǎn)云，然后用flann算法去查找位置

數(shù)據(jù)庫(kù)設(shè)計(jì)要保證增加，刪除，插入和維護(hù)kd_tree的速度足夠快，更好的提供云服務(wù)

抽樣和特征提取可以加快查找速度，減少點(diǎn)云地圖的數(shù)據(jù)大小

daohu527/Dig-into-Apollo?github.com

參考資料

地圖制作

One way Google’s cars localize themselves - Mapping Ignorance

點(diǎn)云合并

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

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。