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

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

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

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

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

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

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

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

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

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

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

SLAM分類

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

大部分的無人車導(dǎo)航架構(gòu)就是上圖的架構(gòu)，核心就是NDT，即點云配準(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á)點云掃描配準(zhǔn)算法。ICP和NDT都比較消耗CPU資源，而不是GPU或AI資源。NDT是目前最常用的無人車定位算法，在無人車主流操作系統(tǒng)AUTOWARE中直接嵌入了完整的NDT算法。

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

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

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

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

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

NDT流程

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

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

審核編輯 ：李倩