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

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

無人車的自車坐標(biāo)系

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

以百度Apollo提供的自車坐標(biāo)系為例，自車坐標(biāo)系的定義為：

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

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

x軸–自車面向前方時，指向車輛右側(cè)

車輛坐標(biāo)系的原點(diǎn)在車輛后輪軸的中心，如下圖所示。

傳感器坐標(biāo)系與自車坐標(biāo)系的轉(zhuǎn)換

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

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

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

綠點(diǎn)轉(zhuǎn)換到自車坐標(biāo)系下需要經(jīng)過一定的數(shù)學(xué)運(yùn)算?；舅悸肥沁@樣的：

|平移

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

|旋轉(zhuǎn)

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

坐標(biāo)系的平移和旋轉(zhuǎn)是兩件相互獨(dú)立的事情，先平移再旋轉(zhuǎn)和先旋轉(zhuǎn)再平移并不會影響最終的結(jié)果。

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

|平移

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

|旋轉(zhuǎn)

繞軸旋轉(zhuǎn)需要引入角度，不是簡單的加減運(yùn)算，所以我們通過圖示來推導(dǎo)一下。

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

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

通過做輔助線進(jìn)行計算，如下圖藍(lán)線所示所示：

何關(guān)系可用以下兩個等式表示：

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

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

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

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

角雷達(dá)目標(biāo)的坐標(biāo)依次右乘這三個矩陣，就完成了沿著Z軸，Y軸，X軸的旋轉(zhuǎn)，得到的結(jié)果就是自車坐標(biāo)系下的坐標(biāo)值了。即

再加上一個平移的矩陣，就能夠完整描述整個坐標(biāo)轉(zhuǎn)換的關(guān)系了

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

歐拉旋轉(zhuǎn)所存在的問題

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

視頻出處：

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

結(jié)語

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

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