無(wú)論是高級(jí)輔助駕駛ADAS系統(tǒng)，還是自動(dòng)駕駛系統(tǒng)，一個(gè)必不可少的重要工作就是計(jì)算自己的位置，以及自己與道路、車(chē)輛、行人等交通元素之間的相對(duì)位置關(guān)系、速度關(guān)系。為了描述這些復(fù)雜的空間、時(shí)間關(guān)系，建立一套時(shí)空坐標(biāo)體系，是實(shí)現(xiàn)各種環(huán)境感知、決策規(guī)劃算法的前提條件。

下面我們介紹自動(dòng)駕駛技術(shù)中幾種常用的坐標(biāo)系統(tǒng)，以及他們之間如何完成關(guān)聯(lián)和轉(zhuǎn)換，最終構(gòu)建出統(tǒng)一的環(huán)境模型。

所謂時(shí)空坐標(biāo)系，包括三維空間坐標(biāo)系和一維時(shí)間坐標(biāo)系。在此基礎(chǔ)上，用解析的形式（坐標(biāo)）把物體在空間和時(shí)間的位置、姿態(tài)表示出來(lái)。一般三維空間坐標(biāo)系用三個(gè)正交軸X，Y，Z表示物體的位置，用繞這三個(gè)正交軸的旋轉(zhuǎn)角度（roll 橫滾角, pitch 俯仰角, yaw 偏航角）表示物體的姿態(tài)。時(shí)間坐標(biāo)系只有一個(gè)維度。為了表述方便，我們一般將空間坐標(biāo)和時(shí)間坐標(biāo)分開(kāi)討論。

01 攝像機(jī)坐標(biāo)系統(tǒng)

攝像機(jī)/攝像頭以其低廉的價(jià)格、豐富的圖像信息，成為自動(dòng)駕駛中最受人們關(guān)注的傳感器之一。攝像頭的作用是把三維世界中的形狀、顏色信息，壓縮到一張二維圖像上?；跀z像頭的感知算法則是從二維圖像中提取并還原三維世界中的元素和信息，如車(chē)道線，車(chē)輛、行人等，并計(jì)算他們與自己的相對(duì)位置。

圖像坐標(biāo)系

電腦上存儲(chǔ)的照片或圖像，一般以左上角為原點(diǎn)，向右為x正方向，向下為y正方向，單位以“像素”最為常用。圖像坐標(biāo)系為二維坐標(biāo)系，標(biāo)記為(Xi, Yi)。

攝像機(jī)坐標(biāo)系

由于圖像坐標(biāo)系向右為x，向下為y，所以攝像機(jī)坐標(biāo)系以鏡頭主光軸中心為原點(diǎn)，一般向右為x正方向，向下為y正方向，向前為z正方向。這樣，x，y方向與圖像坐標(biāo)系的方向吻合，z方向即為景深，同時(shí)符合右手坐標(biāo)系的定義，便于算法中的向量計(jì)算。攝像機(jī)坐標(biāo)系記為(Xc, Yc)。

像平面坐標(biāo)系

為了能夠定量描述三維空間到二維圖像的映射關(guān)系，圖形學(xué)里引入了像平面坐標(biāo)系。它是攝像機(jī)坐標(biāo)系的一個(gè)平移，中心仍在攝像機(jī)主光軸上，距離光軸中心的距離等于攝像機(jī)的焦距。

我們知道攝像機(jī)會(huì)在光軸中心后方的底片上成一個(gè)縮小的倒像，是真正的像平面(X’f, Y’f)。但是為了分析和計(jì)算方便，我們會(huì)在光軸中心前方設(shè)立一個(gè)虛擬像平面(Xf, Yf)。虛擬像平面上的成像為正像，大小與真實(shí)倒像相同。

攝像機(jī)坐標(biāo)系統(tǒng)

坐標(biāo)間的轉(zhuǎn)換關(guān)系

簡(jiǎn)單來(lái)講，從攝像機(jī)坐標(biāo)系到像平面坐標(biāo)系存在以下簡(jiǎn)單映射關(guān)系。

其中f為攝像機(jī)焦距。

從以毫米為單位的像平面坐標(biāo)系到以像素為單位的圖像坐標(biāo)系，存在線性轉(zhuǎn)換關(guān)系：

其中sx，sy是圖像上每個(gè)像素在像平面上所對(duì)應(yīng)的物理尺寸，單位是像素/毫米。(Cx, Cy)是像平面中心在圖像中的位置，單位是像素。

攝像機(jī)的內(nèi)參和外參

攝像機(jī)的焦距f, 像素尺寸sx，sy，和圖像中成像中心的位置(Cx, Cy)在計(jì)算機(jī)圖形學(xué)中被稱(chēng)為攝像機(jī)的內(nèi)部參數(shù)，簡(jiǎn)稱(chēng)內(nèi)參，用來(lái)確定攝像機(jī)從三維空間到二維圖像的投影關(guān)系。實(shí)際應(yīng)用中攝像機(jī)的內(nèi)參會(huì)更為復(fù)雜，還包括圖像的畸變率等參數(shù)。在自動(dòng)駕駛應(yīng)用中，攝像機(jī)的內(nèi)參為常數(shù)，使用中不會(huì)發(fā)生變化，但需要在使用前做好標(biāo)定工作。

攝像機(jī)的拍攝過(guò)程，可以抽象成是從三維攝像機(jī)坐標(biāo)系映射到二維像平面坐標(biāo)系，再映射到圖像坐標(biāo)系的過(guò)程。圖像感知算法則是這一過(guò)程的逆過(guò)程，通過(guò)二維圖像推斷物體在三維攝像機(jī)坐標(biāo)系中的位置，例如獲得距離（深度）信息。

從二維圖像中獲取三維距離信息

如果需要獲得物體在世界坐標(biāo)系中的位置，則還需要知道攝像機(jī)在世界坐標(biāo)系中的位姿。這一位姿表示被稱(chēng)為攝像機(jī)的外部參數(shù)，簡(jiǎn)稱(chēng)外參，用來(lái)決定攝像機(jī)坐標(biāo)與世界坐標(biāo)系之間相對(duì)位置關(guān)系。自動(dòng)駕駛應(yīng)用中，得到這一位置關(guān)系還需要一系列的標(biāo)定和定位工作。在后邊的坐標(biāo)系關(guān)聯(lián)中進(jìn)行介紹。

02 激光雷達(dá)坐標(biāo)系系統(tǒng)

激光雷達(dá)是自動(dòng)駕駛，特別是無(wú)人駕駛中最重要的傳感器之一。目前世界上幾乎所有L4級(jí)別以上的自動(dòng)駕駛試驗(yàn)測(cè)試車(chē)都配備了不同型號(hào)的激光雷達(dá)。

激光雷達(dá)的基本原理非常簡(jiǎn)單。多線激光雷達(dá)基本上可以看做是按一定角度綁在一起，并且不停旋轉(zhuǎn)的高速激光測(cè)距儀。從激光雷達(dá)的英文名（Lidar, Light Detection and Ranging, 光學(xué)檢測(cè)與測(cè)距）中也可以看出，它的最基本功能就是測(cè)距，只不過(guò)它測(cè)距的速度非?？?。

以Velodyne生產(chǎn)的64線激光雷達(dá)HDL64為例，其在垂直方向上可以幾乎同時(shí)在64個(gè)方向上發(fā)射激光。根據(jù)反射回的激光在空中的飛行時(shí)間（TOF, time of fly）就可以計(jì)算出激光雷達(dá)距離物體表面的距離。這64束垂直分布的激光，隨上部機(jī)體一起旋轉(zhuǎn)，從而完成對(duì)環(huán)境360度的掃描。大量的數(shù)據(jù)點(diǎn)繪制在3維空間中，形成了云狀分布，被稱(chēng)為激光點(diǎn)云（Point Cloud）。

64線激光雷達(dá)進(jìn)行環(huán)境掃描過(guò)程
（上：?jiǎn)螏瑨呙钄?shù)據(jù)；下：全周掃描數(shù)據(jù)）

單束激光的測(cè)距結(jié)果是一個(gè)1維數(shù)據(jù)，我們需要建立三維坐標(biāo)系，并對(duì)原始測(cè)距數(shù)據(jù)進(jìn)行轉(zhuǎn)換，才能得到空間中的三維點(diǎn)云數(shù)據(jù)。

如下圖，旋轉(zhuǎn)式激光雷達(dá)一般選擇激光發(fā)射中心作為坐標(biāo)系原點(diǎn)，向上為Z軸正方向，X軸Y軸構(gòu)成水平平面。圖中紅色線條為激光雷達(dá)發(fā)出的激光束，在任意靜止時(shí)刻形成平行于Z軸，垂直于XY平面的扇形掃描區(qū)。每束出射激光在豎直方向上的俯仰角θi為固定值，在設(shè)計(jì)制造時(shí)確定，屬于激光雷達(dá)的內(nèi)部參數(shù)。扇形掃描平面繞Z軸旋轉(zhuǎn)的角度φ(t)隨時(shí)間變化，并會(huì)在原始測(cè)量數(shù)據(jù)中給出。

激光雷達(dá)坐標(biāo)系統(tǒng)

例如，第i束激光在某t0時(shí)刻照射到某物體表面的P點(diǎn)，測(cè)距結(jié)果顯示P點(diǎn)與激光雷達(dá)間的距離為L(zhǎng)，則該測(cè)量點(diǎn)P的原始測(cè)量數(shù)據(jù)可以極坐標(biāo)形式(φ(t0), θi, L )來(lái)表示。同時(shí)，P點(diǎn)在激光雷達(dá)的正交坐標(biāo)系(XL,YL,ZL)中表示為P(xL,yL,zL)，存在如下轉(zhuǎn)換關(guān)系。用正交坐標(biāo)系表示的點(diǎn)云數(shù)據(jù)，在實(shí)際中更為常用。

如果需要得到P點(diǎn)在世界坐標(biāo)系中的位置，則還需要一系列的標(biāo)定和定位工作。在后邊的坐標(biāo)系對(duì)齊中進(jìn)行介紹。

03 車(chē)體坐標(biāo)系系統(tǒng)

車(chē)體坐標(biāo)系用來(lái)描述車(chē)輛周?chē)奈矬w和本車(chē)之間的相對(duì)位置關(guān)系。目前學(xué)術(shù)界和工業(yè)界有幾種比較常用的車(chē)體坐標(biāo)系定義方式。分別是ISO國(guó)際標(biāo)準(zhǔn)定義，SAE（Society of Automotive Engineers）汽車(chē)工程師協(xié)會(huì)定義，和基于慣性測(cè)量單元IMU的坐標(biāo)定義。

在車(chē)輛動(dòng)力學(xué)分析中，ISO定義的車(chē)體坐標(biāo)系較為常見(jiàn)。SAE定義的車(chē)體坐標(biāo)系與航空航天領(lǐng)域常用的機(jī)體坐標(biāo)系相一致。基于IMU定義的車(chē)體坐標(biāo)系，則在IMU的相關(guān)應(yīng)用中較為常見(jiàn)。無(wú)論使用哪一種坐標(biāo)系定義，只要使用正確，都可以完成對(duì)車(chē)身位姿的描述，以及確定周?chē)矬w和本車(chē)間的相對(duì)位置關(guān)系。研發(fā)人員可以根據(jù)應(yīng)用需求和使用習(xí)慣來(lái)選擇車(chē)體坐標(biāo)系。

多種車(chē)體坐標(biāo)系定義舉例

04 世界坐標(biāo)系系統(tǒng)

前面介紹的幾種坐標(biāo)系都是基于設(shè)備的局部坐標(biāo)系，世界坐標(biāo)系則是一個(gè)描述地球上位置關(guān)系的系統(tǒng)。地球是一個(gè)不規(guī)則的橢球，描述地球表面上的相位關(guān)系就沒(méi)有那么直接。人們?cè)缭?5世紀(jì)就開(kāi)始了對(duì)地球上幾何位置關(guān)系的研究，并且開(kāi)始了世界地圖的繪制。到了現(xiàn)代，人們已經(jīng)制定了多種世界坐標(biāo)系統(tǒng)作為國(guó)際標(biāo)準(zhǔn)，用來(lái)描述地球上的絕對(duì)位置和相對(duì)位置關(guān)系。

古老的世界地圖

WGS-84經(jīng)緯坐標(biāo)系

WGS-84坐標(biāo)系一種國(guó)際上采用的地心坐標(biāo)系。坐標(biāo)原點(diǎn)為地球質(zhì)心，其地心空間直角坐標(biāo)系的Z軸指向BIH （國(guó)際時(shí)間服務(wù)機(jī)構(gòu)）1984年定義的協(xié)議地球極(CTP)方向，X軸指向零子午面和CTP赤道的交點(diǎn)，Y軸與Z軸、X軸垂直構(gòu)成右手坐標(biāo)系，稱(chēng)為1984年世界大地坐標(biāo)系統(tǒng)。GPS廣播星歷是以WGS-84坐標(biāo)系為根據(jù)的。

WGS-84坐標(biāo)系采用大地經(jīng)度、緯度、和大地高程來(lái)描述地球上任意一點(diǎn)的位置。經(jīng)緯線相互交織構(gòu)成經(jīng)緯網(wǎng)，用經(jīng)度、緯度表示地面上點(diǎn)的位置就是地理坐標(biāo)。

用經(jīng)緯度表示的大地坐標(biāo)是一種橢球面上的坐標(biāo)，不能直接應(yīng)用于測(cè)繪。因此需要將他們按一定的數(shù)學(xué)規(guī)律轉(zhuǎn)換為平面直角坐標(biāo)。在平面直角坐標(biāo)系中我們能較方便地計(jì)算地表（公路上）兩個(gè)物體的相對(duì)距離和位置關(guān)系。

WGS-84 坐標(biāo)系定義

UTM坐標(biāo)系

在測(cè)繪（例如高精地圖的繪制）和導(dǎo)航（例如無(wú)人車(chē)的導(dǎo)航）中，常常需要用米為單位表示距離和大小。然而GPS的定位結(jié)果所使用的wgs84坐標(biāo)卻是用經(jīng)緯度表示位置。于是需要一種坐標(biāo)轉(zhuǎn)換或者映射關(guān)系將經(jīng)緯度坐標(biāo)轉(zhuǎn)換為以米為單位的平面直角坐標(biāo)。

目前，這種坐標(biāo)映射關(guān)系有多種標(biāo)準(zhǔn)，比如國(guó)際上通用的UTM坐標(biāo)系，我國(guó)的北京54坐標(biāo)系和西安80坐標(biāo)系。其基本思想都是把橢球形的地球表面按照小的區(qū)塊展開(kāi)，投影到一個(gè)曲面（圓柱面或橢圓柱面）上，曲面再次展開(kāi)鋪平成平面，進(jìn)而構(gòu)成平面直角坐標(biāo)系。這里對(duì)國(guó)際上較為常用的UTM坐標(biāo)系做一個(gè)簡(jiǎn)單介紹。

UTM(Universal Transverse Mercartor, 通用橫軸橫墨卡托)坐標(biāo)系統(tǒng)使用UTM投影將橢球面分區(qū)塊映射到平面直角坐標(biāo)系中。這種坐標(biāo)系統(tǒng)及其所依據(jù)的投影已經(jīng)廣泛用于地形圖，作為影像和自然資源數(shù)據(jù)庫(kù)的參考格網(wǎng)以及要求精確定位的其他應(yīng)用。

UTM投影是等角橫軸割圓柱投影，圓柱割地球于南緯80度、北緯84度兩條等高圈，之間的地球表面積按經(jīng)度6度劃分為南北縱帶（投影帶）。從180度經(jīng)線開(kāi)始向東將這些投影帶編號(hào)，從1編至60(北京處于第50帶）。UTM投影沿每一條南北格網(wǎng)線比例系數(shù)為常數(shù)，在東西方向則為變數(shù)，中心格網(wǎng)線的比例系數(shù)為0.9996，在南北縱行最寬部分的邊緣上距離中心點(diǎn)大約 363公里，比例系數(shù)為 1.00158。

橫軸墨卡托投影示意圖

如果把投影中的割圓柱平行于地軸放置，則為正軸墨卡托投影。以正軸墨卡托投影得到的平面直角坐標(biāo)系下世界地圖，其比例尺隨緯度變化而變化。赤道的比例尺最小，比例尺隨緯度升高急劇變大。比如在正軸摩卡多投影地圖上，位于高緯地區(qū)的格林蘭島（面積216萬(wàn)平方公里）看上去面積要比澳大利亞（面積769萬(wàn)平方公里）還大得多。

正軸墨卡托投影示意圖

05 坐標(biāo)系間的關(guān)聯(lián)

前邊介紹了用于自動(dòng)駕駛的傳感器局部坐標(biāo)系，車(chē)體坐標(biāo)系，以及基于大地坐標(biāo)的世界坐標(biāo)系。在實(shí)際應(yīng)用中，我們需要通過(guò)不同傳感器確定障礙物與自動(dòng)駕駛汽車(chē)間的相對(duì)位置，同時(shí)需要知道本車(chē)在世界坐標(biāo)和地圖中的位置與姿態(tài)。這就需要將不同的坐標(biāo)系關(guān)聯(lián)起來(lái)，并建立他們之間的轉(zhuǎn)換關(guān)系。

自動(dòng)駕駛系統(tǒng)中的多個(gè)坐標(biāo)系

從傳感器坐標(biāo)系到車(chē)體坐標(biāo)系

自動(dòng)駕駛汽車(chē)一般都裝有多個(gè)傳感器，每個(gè)傳感器安裝的位置、方向都不一樣。同一個(gè)目標(biāo)（如車(chē)輛、行人）在各個(gè)傳感器視野中出現(xiàn)的位置也都不同。為了將不同傳感器間彼此獨(dú)立的結(jié)果關(guān)聯(lián)起來(lái)，建立統(tǒng)一的環(huán)境模型，我們需要找到各個(gè)傳感器與車(chē)體間的位置關(guān)系，這也是自動(dòng)駕駛中感知融合算法的最基本步驟。傳感器在車(chē)體上的安裝位置一旦確定，在運(yùn)行中就會(huì)保持固定，所以可以采用離線標(biāo)定的方法確定各傳感器相對(duì)車(chē)體的精確位置。

傳感器離線標(biāo)定的方法有很多，這里不一一介紹。算法的總體思想是通過(guò)調(diào)整各坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，使同一個(gè)物體（如下圖中的棋盤(pán)格標(biāo)定板）通過(guò)多個(gè)傳感器感知得到的獨(dú)立結(jié)果，經(jīng)過(guò)坐標(biāo)系轉(zhuǎn)換后，可以在車(chē)體坐標(biāo)系下精確吻合，形成統(tǒng)一結(jié)果。

傳感器標(biāo)定及標(biāo)定板示例

從車(chē)體坐標(biāo)系到世界坐標(biāo)系

車(chē)體坐標(biāo)系和世界坐標(biāo)系之間的關(guān)系是由車(chē)輛本身的位置和姿態(tài)決定的，這一轉(zhuǎn)換關(guān)系可以從車(chē)輛的定位結(jié)果中直接得到。通過(guò)車(chē)體和世界坐標(biāo)系的轉(zhuǎn)換關(guān)系，可以確定車(chē)體在高精地圖中的位置和方向，進(jìn)而可以計(jì)算出車(chē)體和其他道路元素，例如車(chē)道線、紅綠燈、停止線，之間的相對(duì)關(guān)系。

06 時(shí)間坐標(biāo)系統(tǒng)

自動(dòng)駕駛應(yīng)用所應(yīng)對(duì)的是一個(gè)隨時(shí)間變化的環(huán)境，所以時(shí)間坐標(biāo)系統(tǒng)的設(shè)立于統(tǒng)一也是至關(guān)重要的一環(huán)。自動(dòng)駕駛中一般使用多種不同類(lèi)型的傳感器，彼此獨(dú)立地對(duì)環(huán)境進(jìn)行感知。這樣會(huì)造成各傳感器收集的環(huán)境數(shù)據(jù)并不在同一個(gè)時(shí)間點(diǎn)。即便空間坐標(biāo)系已經(jīng)建立了完美的轉(zhuǎn)換關(guān)系，在時(shí)間上也無(wú)法將環(huán)境數(shù)據(jù)進(jìn)行統(tǒng)一。所以除了空間坐標(biāo)系需要進(jìn)行精確標(biāo)定外，各個(gè)設(shè)備之間的時(shí)間坐標(biāo)系也需要進(jìn)行同步。

統(tǒng)一的時(shí)間系統(tǒng)

自動(dòng)駕駛系統(tǒng)中含有多個(gè)主機(jī)、傳感器、和控制器，一般都具有自己獨(dú)立的時(shí)鐘。為了建立統(tǒng)一的時(shí)間坐標(biāo)系統(tǒng)，讓各個(gè)設(shè)備使用相同的時(shí)間基準(zhǔn)，一個(gè)高精度授時(shí)系統(tǒng)是必不可少的。

自動(dòng)駕駛中一般采用GPS的時(shí)鐘系統(tǒng)作為各個(gè)系統(tǒng)的時(shí)間基準(zhǔn)。GPS時(shí)間系統(tǒng)規(guī)定1980年1月6日零時(shí)為時(shí)間坐標(biāo)系的原點(diǎn)，時(shí)間向上累加，系統(tǒng)授時(shí)精度可以達(dá)到納秒量級(jí)。同時(shí)自動(dòng)駕駛中所使用的大部分設(shè)備都具備接受外部授時(shí)的功能。以Velodyne激光雷達(dá)為例，設(shè)備可以接收標(biāo)準(zhǔn)的PPS（Pulse per Second，秒脈沖）和NMEA報(bào)文（一種串口通訊格式）的授時(shí)方法。

硬件同步觸發(fā)

一些設(shè)備的數(shù)據(jù)采集可以通過(guò)外部觸發(fā)的方式進(jìn)行激活，于是可以使用同一個(gè)外部信號(hào)，同時(shí)激活多個(gè)傳感器，從而得到同一個(gè)時(shí)間點(diǎn)上的環(huán)境信息。

例如攝像機(jī)的曝光可以通過(guò)外部開(kāi)關(guān)信號(hào)進(jìn)行觸發(fā)，于是自動(dòng)駕駛汽車(chē)上的多個(gè)攝像機(jī)可以使用同一個(gè)開(kāi)關(guān)信號(hào)進(jìn)行曝光和采樣的硬同步。進(jìn)而，這一開(kāi)關(guān)信號(hào)還可以與激光雷達(dá)等其他傳感器進(jìn)行協(xié)同，完成不同種類(lèi)傳感器間的同步觸發(fā)操作。

軟件時(shí)間對(duì)齊

另一些傳感器的采樣不支持外部觸發(fā)，同時(shí)有些設(shè)備的工作頻率也不一致，無(wú)法做到嚴(yán)格的硬時(shí)間同步，這就需要在軟件中進(jìn)行處理。

有了前面提到的統(tǒng)一的時(shí)間系統(tǒng)，通過(guò)不同傳感器獲得的環(huán)境信息即便不在同一個(gè)時(shí)間點(diǎn)上，也有著統(tǒng)一的時(shí)間標(biāo)記。這樣通過(guò)軟件計(jì)算，對(duì)非同步采樣結(jié)果進(jìn)行差值或外推，就可以近似得到同一個(gè)時(shí)間點(diǎn)上的環(huán)境信息，成為決策控制系統(tǒng)進(jìn)行判斷的依據(jù)。