無論是高級輔助駕駛ADAS系統，還是自動駕駛系統，一個必不可少的重要工作就是計算自己的位置，以及自己與道路、車輛、行人等交通元素之間的相對位置關系、速度關系。為了描述這些復雜的空間、時間關系，建立一套時空坐標體系，是實現各種環境感知、決策規劃算法的前提條件。

下面我們介紹自動駕駛技術中幾種常用的坐標系統，以及他們之間如何完成關聯和轉換，最終構建出統一的環境模型。

所謂時空坐標系，包括三維空間坐標系和一維時間坐標系。在此基礎上，用解析的形式（坐標）把物體在空間和時間的位置、姿態表示出來。一般三維空間坐標系用三個正交軸X，Y，Z表示物體的位置，用繞這三個正交軸的旋轉角度（roll 橫滾角, pitch 俯仰角, yaw 偏航角）表示物體的姿態。時間坐標系只有一個維度。為了表述方便，我們一般將空間坐標和時間坐標分開討論。

01 攝像機坐標系統

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

圖像坐標系

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

攝像機坐標系

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

像平面坐標系

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

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

攝像機坐標系統

坐標間的轉換關系

簡單來講，從攝像機坐標系到像平面坐標系存在以下簡單映射關系。

其中f為攝像機焦距。

從以毫米為單位的像平面坐標系到以像素為單位的圖像坐標系，存在線性轉換關系：

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

攝像機的內參和外參

攝像機的焦距f, 像素尺寸sx，sy，和圖像中成像中心的位置(Cx, Cy)在計算機圖形學中被稱為攝像機的內部參數，簡稱內參，用來確定攝像機從三維空間到二維圖像的投影關系。實際應用中攝像機的內參會更為復雜，還包括圖像的畸變率等參數。在自動駕駛應用中，攝像機的內參為常數，使用中不會發生變化，但需要在使用前做好標定工作。

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

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

如果需要獲得物體在世界坐標系中的位置，則還需要知道攝像機在世界坐標系中的位姿。這一位姿表示被稱為攝像機的外部參數，簡稱外參，用來決定攝像機坐標與世界坐標系之間相對位置關系。自動駕駛應用中，得到這一位置關系還需要一系列的標定和定位工作。在后邊的坐標系關聯中進行介紹。

02 激光雷達坐標系系統

激光雷達是自動駕駛，特別是無人駕駛中最重要的傳感器之一。目前世界上幾乎所有L4級別以上的自動駕駛試驗測試車都配備了不同型號的激光雷達。

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

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

64線激光雷達進行環境掃描過程
（上：單幀掃描數據；下：全周掃描數據）

單束激光的測距結果是一個1維數據，我們需要建立三維坐標系，并對原始測距數據進行轉換，才能得到空間中的三維點云數據。

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

激光雷達坐標系統

例如，第i束激光在某t0時刻照射到某物體表面的P點，測距結果顯示P點與激光雷達間的距離為L，則該測量點P的原始測量數據可以極坐標形式(φ(t0), θi, L )來表示。同時，P點在激光雷達的正交坐標系(XL,YL,ZL)中表示為P(xL,yL,zL)，存在如下轉換關系。用正交坐標系表示的點云數據，在實際中更為常用。

如果需要得到P點在世界坐標系中的位置，則還需要一系列的標定和定位工作。在后邊的坐標系對齊中進行介紹。

03 車體坐標系系統

車體坐標系用來描述車輛周圍的物體和本車之間的相對位置關系。目前學術界和工業界有幾種比較常用的車體坐標系定義方式。分別是ISO國際標準定義，SAE（Society of Automotive Engineers）汽車工程師協會定義，和基于慣性測量單元IMU的坐標定義。

在車輛動力學分析中，ISO定義的車體坐標系較為常見。SAE定義的車體坐標系與航空航天領域常用的機體坐標系相一致。基于IMU定義的車體坐標系，則在IMU的相關應用中較為常見。無論使用哪一種坐標系定義，只要使用正確，都可以完成對車身位姿的描述，以及確定周圍物體和本車間的相對位置關系。研發人員可以根據應用需求和使用習慣來選擇車體坐標系。

多種車體坐標系定義舉例

04 世界坐標系系統

前面介紹的幾種坐標系都是基于設備的局部坐標系，世界坐標系則是一個描述地球上位置關系的系統。地球是一個不規則的橢球，描述地球表面上的相位關系就沒有那么直接。人們早在15世紀就開始了對地球上幾何位置關系的研究，并且開始了世界地圖的繪制。到了現代，人們已經制定了多種世界坐標系統作為國際標準，用來描述地球上的絕對位置和相對位置關系。

古老的世界地圖

WGS-84經緯坐標系

WGS-84坐標系一種國際上采用的地心坐標系。坐標原點為地球質心，其地心空間直角坐標系的Z軸指向BIH （國際時間服務機構）1984年定義的協議地球極(CTP)方向，X軸指向零子午面和CTP赤道的交點，Y軸與Z軸、X軸垂直構成右手坐標系，稱為1984年世界大地坐標系統。GPS廣播星歷是以WGS-84坐標系為根據的。

WGS-84坐標系采用大地經度、緯度、和大地高程來描述地球上任意一點的位置。經緯線相互交織構成經緯網，用經度、緯度表示地面上點的位置就是地理坐標。

用經緯度表示的大地坐標是一種橢球面上的坐標，不能直接應用于測繪。因此需要將他們按一定的數學規律轉換為平面直角坐標。在平面直角坐標系中我們能較方便地計算地表（公路上）兩個物體的相對距離和位置關系。

WGS-84 坐標系定義

UTM坐標系

在測繪（例如高精地圖的繪制）和導航（例如無人車的導航）中，常常需要用米為單位表示距離和大小。然而GPS的定位結果所使用的wgs84坐標卻是用經緯度表示位置。于是需要一種坐標轉換或者映射關系將經緯度坐標轉換為以米為單位的平面直角坐標。

目前，這種坐標映射關系有多種標準，比如國際上通用的UTM坐標系，我國的北京54坐標系和西安80坐標系。其基本思想都是把橢球形的地球表面按照小的區塊展開，投影到一個曲面（圓柱面或橢圓柱面）上，曲面再次展開鋪平成平面，進而構成平面直角坐標系。這里對國際上較為常用的UTM坐標系做一個簡單介紹。

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

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

橫軸墨卡托投影示意圖

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

正軸墨卡托投影示意圖

05 坐標系間的關聯

前邊介紹了用于自動駕駛的傳感器局部坐標系，車體坐標系，以及基于大地坐標的世界坐標系。在實際應用中，我們需要通過不同傳感器確定障礙物與自動駕駛汽車間的相對位置，同時需要知道本車在世界坐標和地圖中的位置與姿態。這就需要將不同的坐標系關聯起來，并建立他們之間的轉換關系。

自動駕駛系統中的多個坐標系

從傳感器坐標系到車體坐標系

自動駕駛汽車一般都裝有多個傳感器，每個傳感器安裝的位置、方向都不一樣。同一個目標（如車輛、行人）在各個傳感器視野中出現的位置也都不同。為了將不同傳感器間彼此獨立的結果關聯起來，建立統一的環境模型，我們需要找到各個傳感器與車體間的位置關系，這也是自動駕駛中感知融合算法的最基本步驟。傳感器在車體上的安裝位置一旦確定，在運行中就會保持固定，所以可以采用離線標定的方法確定各傳感器相對車體的精確位置。

傳感器離線標定的方法有很多，這里不一一介紹。算法的總體思想是通過調整各坐標系之間的轉換關系，使同一個物體（如下圖中的棋盤格標定板）通過多個傳感器感知得到的獨立結果，經過坐標系轉換后，可以在車體坐標系下精確吻合，形成統一結果。

傳感器標定及標定板示例

從車體坐標系到世界坐標系

車體坐標系和世界坐標系之間的關系是由車輛本身的位置和姿態決定的，這一轉換關系可以從車輛的定位結果中直接得到。通過車體和世界坐標系的轉換關系，可以確定車體在高精地圖中的位置和方向，進而可以計算出車體和其他道路元素，例如車道線、紅綠燈、停止線，之間的相對關系。

06 時間坐標系統

自動駕駛應用所應對的是一個隨時間變化的環境，所以時間坐標系統的設立于統一也是至關重要的一環。自動駕駛中一般使用多種不同類型的傳感器，彼此獨立地對環境進行感知。這樣會造成各傳感器收集的環境數據并不在同一個時間點。即便空間坐標系已經建立了完美的轉換關系，在時間上也無法將環境數據進行統一。所以除了空間坐標系需要進行精確標定外，各個設備之間的時間坐標系也需要進行同步。

統一的時間系統

自動駕駛系統中含有多個主機、傳感器、和控制器，一般都具有自己獨立的時鐘。為了建立統一的時間坐標系統，讓各個設備使用相同的時間基準，一個高精度授時系統是必不可少的。

自動駕駛中一般采用GPS的時鐘系統作為各個系統的時間基準。GPS時間系統規定1980年1月6日零時為時間坐標系的原點，時間向上累加，系統授時精度可以達到納秒量級。同時自動駕駛中所使用的大部分設備都具備接受外部授時的功能。以Velodyne激光雷達為例，設備可以接收標準的PPS（Pulse per Second，秒脈沖）和NMEA報文（一種串口通訊格式）的授時方法。

硬件同步觸發

一些設備的數據采集可以通過外部觸發的方式進行激活，于是可以使用同一個外部信號，同時激活多個傳感器，從而得到同一個時間點上的環境信息。

例如攝像機的曝光可以通過外部開關信號進行觸發，于是自動駕駛汽車上的多個攝像機可以使用同一個開關信號進行曝光和采樣的硬同步。進而，這一開關信號還可以與激光雷達等其他傳感器進行協同，完成不同種類傳感器間的同步觸發操作。

軟件時間對齊

另一些傳感器的采樣不支持外部觸發，同時有些設備的工作頻率也不一致，無法做到嚴格的硬時間同步，這就需要在軟件中進行處理。

有了前面提到的統一的時間系統，通過不同傳感器獲得的環境信息即便不在同一個時間點上，也有著統一的時間標記。這樣通過軟件計算，對非同步采樣結果進行差值或外推，就可以近似得到同一個時間點上的環境信息，成為決策控制系統進行判斷的依據。