激光雷達(dá)除了感知外，另外一個(gè)主要的用途就是定位，目前的無人出租車研發(fā)基本都是用激光雷達(dá)定位，當(dāng)然嚴(yán)格地說是融合定位。

首先解釋下什么是定位，為什么需要高精度定位。

定位就是無人車知道自己在坐標(biāo)系中的位置和姿態(tài)，這個(gè)坐標(biāo)系可以是一個(gè)局部的坐標(biāo)系，比如一個(gè)園區(qū)，采集這個(gè)園區(qū)的一些地圖，自由定一個(gè)原點(diǎn)，這個(gè)局部坐標(biāo)系便已經(jīng)建好，相對(duì)于這個(gè)坐標(biāo)系來得到車輛的位置和姿態(tài)。坐標(biāo)系也可以是一個(gè)全局的坐標(biāo)系，比如全球坐標(biāo)系，可以知道一個(gè)很精確的位置。這就是常說的經(jīng)緯度絕對(duì)定位。位置對(duì)應(yīng)X，Y，Z，即相當(dāng)于某個(gè)坐標(biāo)系，汽車的平移是多少。

姿態(tài)是三個(gè)方向的旋轉(zhuǎn)狀態(tài)，一般會(huì)用歐拉角來表示。包括橫滾、俯仰和航向，分別相對(duì)于X，Y，Z三個(gè)坐標(biāo)軸。如果本地坐標(biāo)系已經(jīng)定義好，現(xiàn)在有一個(gè)車上的坐標(biāo)系，它相對(duì)于本地坐標(biāo)系的變化（即姿態(tài)的變化），就可用三個(gè)角度來表示，也就是本地坐標(biāo)系的三個(gè)軸和相對(duì)坐標(biāo)系的這三個(gè)軸之間的夾角。

除了位置和姿態(tài)這兩個(gè)維度，自定位系統(tǒng)還要輸出很多信息。除去速度、加速度和角速度外，基于數(shù)學(xué)概率算法的相對(duì)定位的加入，自定位系統(tǒng)還需要對(duì)位置和姿態(tài)加上一個(gè)置信度，而基于衛(wèi)星廣播的絕對(duì)定位準(zhǔn)確率達(dá)到了99%，一般不需要加置信度。

雖然車輛本身的傳感器也能輸出速度、加速度和角速度，但是定位系統(tǒng)基于位置的變化輸出的信息準(zhǔn)確度更高，加速度和角速度是相對(duì)于車體本身的，告知車輛當(dāng)時(shí)瞬時(shí)的加速度和角速度，對(duì)人的駕乘體驗(yàn)非常重要。控制模塊根據(jù)這些信息做一些控制上的優(yōu)化，使人的體感更好。

高精度地圖自然需要高精度定位，全局規(guī)劃也需要高精度定位。

上圖是特斯拉2022年AIDAY上的特斯拉未來的FSD Beta版智能駕駛架構(gòu)圖，自然離不開高精度地圖，現(xiàn)在的FSD版不需要高精度地圖，因?yàn)楝F(xiàn)在的FSD只是L2級(jí)輔助駕駛，沒有車道級(jí)定位，自然也用不上車道級(jí)地圖。

絕對(duì)定位離不開衛(wèi)星或RTK系統(tǒng)，姿態(tài)則主要依靠IMU，相對(duì)定位基于環(huán)境特征匹配，常見的視覺和激光雷達(dá)定位就是如此。姿態(tài)很多時(shí)候可以叫航跡推算，航跡推算就是根據(jù)上一時(shí)刻的“位置”和“姿態(tài)”，疊加一些測(cè)量信息可以知道現(xiàn)在的“位置”和“姿態(tài)”。IMU是慣性測(cè)量單元，包含了加速度計(jì)和陀螺儀，其中加速度計(jì)會(huì)輸出加速度的信息，同時(shí)還包含重力加速度；陀螺儀是一個(gè)旋轉(zhuǎn)，即是前面所講到三個(gè)軸上的一個(gè)旋轉(zhuǎn)。要做到無人車10秒單點(diǎn)水平精度1.41米的IMU，這與軍事用途的IMU要求是同一級(jí)別，價(jià)格近20萬人民幣。

視覺定位置信度很低，頂多可以做個(gè)輔助，核心還是激光雷達(dá)，視覺對(duì)光線變化非常敏感，而室外狀態(tài)下，光線每時(shí)每刻都在變化，例如這次跑過去的光照度和下次跑過去的光照度不一樣，上次檢測(cè)到的特征就無法檢測(cè)到，典型SIFT特征或者別的特征就會(huì)造成定位的失敗。但是有一些特征具有明顯Semantic意義，比如車道線或者旁邊立的這些柱子，紅綠燈的柱子或者紅綠燈本身或者一些交通標(biāo)志之類的，對(duì)于定位而言非常有用。

這里說一下SLAM（即時(shí)定位與地圖構(gòu)建），無人車的定位任務(wù)可以看做輕量級(jí)SLAM，SLAM原本用于機(jī)器人領(lǐng)域，SLAM成熟算法都是采用激光雷達(dá)，極少用視覺。

SLAM分類

上表出自2022年8月論文《Evaluation and comparison of eight popular Lidar and Visual SLAMalgorithms》，毫無疑問視覺SLAM精度與激光雷達(dá)差距巨大，則極容易失敗。量產(chǎn)項(xiàng)目沒有用視覺SLAM的例子。

大部分的無人車導(dǎo)航架構(gòu)就是上圖的架構(gòu)，核心就是NDT，即點(diǎn)云配準(zhǔn)的正態(tài)分布變換算法，它可以做圖也可以定位。無人車導(dǎo)航近似于機(jī)器人的SLAM問題，思路通常都是利用激光雷達(dá)配準(zhǔn)制圖并定位，常見的方法有ICP、NDT、Gaussian fields。還有一些不常用的如Point-based probabilistic registration ，Likelihood-fieldmatching，Quadratic patches。還有基于深度學(xué)習(xí)的比如蒙特卡洛定位算法（MCL）。這些也可以稱之為激光雷達(dá)點(diǎn)云掃描配準(zhǔn)算法。ICP和NDT都比較消耗CPU資源，而不是GPU或AI資源。NDT是目前最常用的無人車定位算法，在無人車主流操作系統(tǒng)AUTOWARE中直接嵌入了完整的NDT算法。

ICP是早期常用的技術(shù)方式，優(yōu)點(diǎn)是對(duì)初始精度要求不高，缺點(diǎn)是首先要剔除不合適的點(diǎn)對(duì)（點(diǎn)對(duì)距離過大、包含邊界點(diǎn)的點(diǎn)對(duì)），其次是基于點(diǎn)對(duì)的配準(zhǔn)，并沒有包含局部形狀的信息，再次是每次迭代都要搜索最近點(diǎn)，計(jì)算代價(jià)高昂，ICP有不少變體算法如八叉樹或IDC。

通過不斷比對(duì)實(shí)時(shí)掃描到的點(diǎn)云和已經(jīng)建好的全局點(diǎn)云地圖，我們就可以持續(xù)獲得當(dāng)前的位置。ICP（迭代最近點(diǎn)）等配準(zhǔn)算法通過對(duì)所有的點(diǎn)或者提取的特征點(diǎn)進(jìn)行匹配配準(zhǔn)以確定當(dāng)前的位置，但是這樣就有一個(gè)問題：我們所處的環(huán)境是在不斷變化的，比如樹木的稀疏程度，或者環(huán)境中車輛及行人的移動(dòng)，乃至固有的測(cè)量誤差，這些都會(huì)導(dǎo)致實(shí)時(shí)掃描到的點(diǎn)云與已建立的點(diǎn)云地圖有些許的差別，從而導(dǎo)致較大匹配誤差。

而NDT可以在很大程序上消除這種不確定性。NDT沒有計(jì)算兩個(gè)點(diǎn)云中點(diǎn)與點(diǎn)之間的差距，而是先將參考點(diǎn)云（即激光雷達(dá)先驗(yàn)地圖）轉(zhuǎn)換為多維變量的正態(tài)分布，如果變換參數(shù)能使得兩幅激光數(shù)據(jù)匹配的很好，那么變換點(diǎn)在參考系中的概率密度將會(huì)很大。因此，可以考慮用優(yōu)化的方法比如牛頓法，求出使得概率密度之和最大的變換參數(shù)，此時(shí)兩幅激光點(diǎn)云數(shù)據(jù)將匹配的最好。

可以這樣來做一個(gè)通俗的理解：NDT把我們所處的三維世界按照一定長度的立方體（比如30cm*30cm*30cm）進(jìn)行了劃分，類似于一個(gè)魔方，網(wǎng)格Grid，與VOXEL近似，每個(gè)立方體內(nèi)并不是存儲(chǔ)一個(gè)或一些確切的點(diǎn)，而且存儲(chǔ)這個(gè)立方體被占據(jù)的概率密度。當(dāng)接收到需要匹配的點(diǎn)云時(shí)，也按照這樣的劃分方式進(jìn)行劃分，然后進(jìn)行配準(zhǔn)。

因此，NDT具有以下的特征：支持更大的地圖，更稠密的點(diǎn)云，相較于ICP等基于點(diǎn)的匹配算法，速度更快且更加容忍環(huán)境的細(xì)微變化。

NDT流程

NDT，Normal Distributions Transform正態(tài)分布變換算法的簡(jiǎn)稱，其是一種統(tǒng)計(jì)學(xué)模型。如果一組隨機(jī)向量滿足正態(tài)分布，那么它的概率密度函數(shù)為：

其中D表示維度，表示均值向量，表示隨機(jī)向量的協(xié)方差矩陣。由于掃描得到的激光點(diǎn)云數(shù)據(jù)點(diǎn)是三維空間點(diǎn)坐標(biāo)，所以需要采用三維正態(tài)分布。NDT能夠通過概率的形式描述點(diǎn)云的分部情況，這有利于減少配準(zhǔn)所需要的時(shí)間。

審核編輯 ：李倩