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

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

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

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

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

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

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

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

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

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

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

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

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

綠點(diǎn)轉(zhuǎn)換到自車坐標(biāo)系下需要經(jīng)過(guò)一定的數(shù)學(xué)運(yùn)算。基本思路是這樣的：

|平移

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

|旋轉(zhuǎn)

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

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

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

|平移

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

|旋轉(zhuǎn)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

視頻出處：

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

結(jié)語(yǔ)

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

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