無人駕駛系統(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)域，同時在機器人、無人機、三維建模等領(lǐng)域也得到了廣泛使用。

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

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

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

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

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

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

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

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

圖片出處：

https://github.com/ApolloAuto/apollo/blob/master/docs/specs/coordination_cn.md

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

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

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

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

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

| 平移

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

| 旋轉(zhuǎn)

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

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

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

| 平移

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

| 旋轉(zhuǎn)

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

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

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

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

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

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

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

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

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

角雷達目標(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ù)實際情況計算旋轉(zhuǎn)矩陣和平移矩陣。

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

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

視頻出處：

為了解決歐拉旋轉(zhuǎn)所帶來的萬向鎖問題，業(yè)界引入了四元數(shù)。四元數(shù)除了解決萬向鎖的問題外，還能在一定程度上簡化計算。因而百度Apollo也選擇了四元數(shù)作為各個傳感器安裝位置和角度的存儲介質(zhì)。有關(guān)四元數(shù)的定義和使用方法這里不做詳細討論，可參看Apollo的Calibration模塊。鏈接如下：

https://github.com/ApolloAuto/apollo/tree/master/modules/calibration

結(jié)語

實際參與到開發(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)域，同時在機器人、無人機、三維建模等領(lǐng)域也得到了廣泛使用。