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

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

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

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

除了位置和姿態這兩個維度，自定位系統還要輸出很多信息。除去速度、加速度和角速度外，基于數學概率算法的相對定位的加入，自定位系統還需要對位置和姿態加上一個置信度，而基于衛星廣播的絕對定位準確率達到了99%，一般不需要加置信度。

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

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

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

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

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

這里說一下SLAM（即時定位與地圖構建），無人車的定位任務可以看做輕量級SLAM，SLAM原本用于機器人領域，SLAM成熟算法都是采用激光雷達，極少用視覺。

SLAM分類

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

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

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

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

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

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

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

NDT流程

NDT，Normal Distributions Transform正態分布變換算法的簡稱，其是一種統計學模型。如果一組隨機向量滿足正態分布，那么它的概率密度函數為：

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

審核編輯 ：李倩