無人駕駛系統(tǒng)是一個多傳感器整合的系統(tǒng)，需要將不同位置的傳感器數(shù)據(jù)統(tǒng)一到一個固定的坐標系——自車坐標系下，才能分析當前無人車所在的道路場景。本文將會集中在如何將傳感器坐標系下的數(shù)據(jù)轉換到自車坐標系下。有關無人駕駛技術中傳感器到車體坐標系的坐標轉換原理及數(shù)學推導，該原理不僅用在無人車領域，同時在機器人、無人機、三維建模等領域也得到了廣泛使用。

無人車上擁有各種各樣的傳感器，每個傳感器的安裝位置和角度又不盡相同。對于傳感器的提供商，開始并不知道傳感器會以什么角度，安裝在什么位置，因此只能根據(jù)傳感器自身建立坐標系。無人駕駛系統(tǒng)是一個多傳感器整合的系統(tǒng)，需要將不同位置的傳感器數(shù)據(jù)統(tǒng)一到一個固定的坐標系——自車坐標系下，才能分析當前無人車所在的道路場景。

無人車的自車坐標系

不同的無人駕駛團隊對于坐標系的定義可能不同，但這并不影響開發(fā)，只要團隊內部達成一致即可。

以百度Apollo提供的自車坐標系為例，自車坐標系的定義為：

z軸–通過車頂垂直于地面指向上方

y軸–在行駛的方向上指向車輛前方

x軸–自車面向前方時，指向車輛右側

車輛坐標系的原點在車輛后輪軸的中心，如下圖所示。

傳感器坐標系與自車坐標系的轉換

對于無人車的傳感器安裝位置，業(yè)內大同小異，比如奧迪A8的傳感器配置如下圖所示：

以安裝在無人車左前方的角雷達（Cornerradar）為例，進行后面的介紹，叫雷達的安裝位置和坐標系如下圖綠線所示。

角雷達檢測到的障礙物如圖中的綠點所示，綠點在雷達坐標系下的坐標為(x1,y1），為了便于理解暫不加入z方向的坐標。

綠點轉換到自車坐標系下需要經(jīng)過一定的數(shù)學運算。基本思路是這樣的：

|平移

先將角雷達坐標系的O點平移到與自車坐標系的O點重合，此時(x1,y1)需要減去兩個坐標系在x和y方向的距離。如下圖所示：

|旋轉

在兩個坐標系的O點重合后，將角雷達坐標系沿著z軸進行一定角度的旋轉，這樣(x1,y1)就轉到了自車坐標系上。這個過程在數(shù)學上稱為歐拉旋轉。

坐標系的平移和旋轉是兩件相互獨立的事情，先平移再旋轉和先旋轉再平移并不會影響最終的結果。

以上是感性的分析過程，下面我們將這整個過程在數(shù)學上實現(xiàn)。

|平移

平移步驟根據(jù)傳感器安裝位置和自車后軸的距離進行計算，僅僅是XYZ三個方向加減運算。

|旋轉

繞軸旋轉需要引入角度，不是簡單的加減運算，所以我們通過圖示來推導一下。

先將兩個坐標系變換到正常的視角，如下圖所示：

障礙物在角雷達坐標系下的坐標為(x1,y1)，假設障礙物在自車坐標系下的坐標為(x0,y0)，需要根據(jù)安裝角度α（可測量），用x1，y1，α這三個已知量表示x0，y0，求得他們的數(shù)學關系。

通過做輔助線進行計算，如下圖藍線所示所示：

何關系可用以下兩個等式表示：

使用矩陣表示，可以簡化表達，可以用一個等式代替兩個等式，是這樣的

于這次旋轉是繞z軸旋轉，因此旋轉前和旋轉后的z值是保持不變的

將z方向的值也放到上面的等式中，即可得到

就意味著，只要把角雷達采集到的障礙物坐標值與上面這個矩陣進行矩陣乘法運算，即可完成沿Z軸的旋轉。在這里我們把這個矩陣叫做Z軸旋轉矩陣RZ，那必然還有沿著X軸和Y軸的旋轉矩陣RX和RY。

角雷達目標的坐標依次右乘這三個矩陣，就完成了沿著Z軸，Y軸，X軸的旋轉，得到的結果就是自車坐標系下的坐標值了。即

再加上一個平移的矩陣，就能夠完整描述整個坐標轉換的關系了

不同的坐標系定義，會有不同的RX，RY和RZ，因此需要根據(jù)實際情況計算旋轉矩陣和平移矩陣。

歐拉旋轉所存在的問題

以上過程也稱為歐拉旋轉，但是歐拉旋轉會有一個不可避免的問題——萬向鎖。歐拉旋轉的過程中在某些特殊情況時，會導致丟失維度。萬向鎖通過文字解釋起來會有點困難，可以看一個講解萬向鎖的小視頻進行了解。

視頻出處：

為了解決歐拉旋轉所帶來的萬向鎖問題，業(yè)界引入了四元數(shù)。四元數(shù)除了解決萬向鎖的問題外，還能在一定程度上簡化計算。因而百度Apollo也選擇了四元數(shù)作為各個傳感器安裝位置和角度的存儲介質。

結語

實際參與到開發(fā)中你才會發(fā)現(xiàn)，自己在大學學習的高數(shù)和線性代數(shù)等都是很重要的數(shù)學工具，而不是僅僅用來考試的。

以上就是有關無人駕駛技術中傳感器到車體坐標系的坐標轉換原理及數(shù)學推導，該原理不僅用在無人車領域，同時在機器人、無人機、三維建模等領域也得到了廣泛使用。