一文講清自動(dòng)駕駛中的時(shí)空坐標(biāo)系

坐標(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é)中被稱為攝像機(jī)的內(nèi)部參數(shù)，簡(jiǎn)稱內(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)系中的位姿。這一位姿表示被稱為攝像機(jī)的外部參數(shù)，簡(jiǎn)稱外參，用來(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)行介紹。

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

激光雷達(dá)是自動(dòng)駕駛，特別是無(wú)人駕駛中最重要的傳感器之一。目前世界上幾乎所有L4級(jí)別以上的自動(dòng)駕駛試驗(yàn)測(cè)試車都配備了不同型號(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維空間中，形成了云狀分布，被稱為激光點(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ù)中給出。

例如，第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)行介紹。

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

古老的世界地圖

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)系，稱為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)系。

UTM坐標(biāo)系

在測(cè)繪（例如高精地圖的繪制）和導(dǎo)航（例如無(wú)人車的導(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)廣泛用于地形圖，作為衛(wèi)星影像和自然資源數(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)系到車體坐標(biāo)系

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

傳感器離線標(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)換后，可以在車體坐標(biāo)系下精確吻合，形成統(tǒng)一結(jié)果。

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

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

時(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)駕駛中一般使用多種不同類型的傳感器，彼此獨(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)駕駛汽車上的多個(gè)攝像機(jī)可以使用同一個(gè)開(kāi)關(guān)信號(hào)進(jìn)行曝光和采樣的硬同步。進(jìn)而，這一開(kāi)關(guān)信號(hào)還可以與激光雷達(dá)等其他傳感器進(jìn)行協(xié)同，完成不同種類傳感器間的同步觸發(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ù)。