深度剖析自動(dòng)駕駛和機(jī)器人定位技術(shù)

通過(guò)對(duì)機(jī)器人定位的概念、方法、要解決的問(wèn)題及常用的手段做了介紹，希望大家讀完能有所收獲。

▍定位問(wèn)題的引入

我們所熟知的機(jī)器人，可以大致分為以下四類：

這四種類型的機(jī)器人可以做組合，如有智能行為的移動(dòng)機(jī)器人，機(jī)器人可以移動(dòng)并做一些操作行為。前兩類機(jī)器人發(fā)展的比較成熟，已經(jīng)大量應(yīng)用在工業(yè)領(lǐng)域，商業(yè)和家庭領(lǐng)域的應(yīng)用比例在近些年也在增長(zhǎng)。其中移動(dòng)機(jī)器人是我們目前關(guān)心的重點(diǎn)，自動(dòng)駕駛也可以歸結(jié)為移動(dòng)機(jī)器人。移動(dòng)機(jī)器人包含無(wú)人機(jī)、水下機(jī)器人、輪式機(jī)器人、履帶式機(jī)器人、仿生足大狗等。

移動(dòng)機(jī)器人關(guān)心的問(wèn)題有三個(gè)：

移動(dòng)機(jī)器人的核心問(wèn)題就是定位問(wèn)題， 后兩個(gè)問(wèn)題都是以定位問(wèn)題為前提才能達(dá)成的。

▍定位的基本概念

了解定位，我們首先要先了解位置。位置這個(gè)術(shù)語(yǔ)其實(shí)不是很準(zhǔn)確，應(yīng)該叫做位姿，包含位置和目標(biāo)體的朝向（姿態(tài)），我們習(xí)慣于用笛卡爾坐標(biāo)系來(lái)表征位姿：

2D平面的位姿

三個(gè)自由度(x,y,rotation)，x,y是2D平面的坐標(biāo)位置，rotation 是偏向角。

3D世界的位姿

六個(gè)自由度（x,y,z,yaw,pitch,roll)。（x,y,z） 是3D立體坐標(biāo)系中的位置坐標(biāo)， yaw(航向角）、pitch(俯仰角）、roll(傾斜角）分別代表目標(biāo)剛體繞z,y,x軸按照順序旋轉(zhuǎn)后的朝向姿態(tài)。

具體到自動(dòng)駕駛和機(jī)器人，它們都是在一個(gè)平面上進(jìn)行移動(dòng)，雖然中間有顛簸，但是我們關(guān)注的是水平面上的位姿，上下的顛簸引起的位姿變更，可以投影到水平面上，這樣定位問(wèn)題其實(shí)是2D位姿的估計(jì)問(wèn)題，與之相對(duì)的如無(wú)人機(jī)就是3D定位問(wèn)題。

理解定位，必須理解的另外一個(gè)概念就是參考基準(zhǔn)，通俗講就是相對(duì)誰(shuí)的位置和姿態(tài)。我們舉生活中的例子，坐公交汽車，我們相對(duì)公交車位置幾乎是不變的，相對(duì)于馬路上的某個(gè)站牌是一直在變化的。更加嚴(yán)謹(jǐn)?shù)亩x是參考幀， 這個(gè)參考幀具化成視覺(jué)效果，是一個(gè)坐標(biāo)系，遵守右手法則，規(guī)定了在空間的零點(diǎn)位置和三叉戟坐標(biāo)軸方向。

在這里有一個(gè)非常有意思的地方，可以細(xì)細(xì)琢磨，定位是研究參考幀與參考幀的相對(duì)位置關(guān)系。當(dāng)我們向別人描述“我在哪里”的時(shí)候，其實(shí)是描述我作為主體的參考幀相對(duì)于某個(gè)地標(biāo)（如***），甚至地球這個(gè)主體參考幀的相對(duì)位姿描述，放到太陽(yáng)系就不對(duì)了。雖然我們描述位置的時(shí)候，常常是忽略掉這個(gè)地球參考幀的，但那是建立在大家都有這個(gè)地球參考幀的共識(shí)下，去討論位置。這里還有一個(gè)引申的概念就是剛體，組成這個(gè)剛體的所有參考幀互相之間的相對(duì)位姿在任何時(shí)刻都是不變的。我們與***的相對(duì)位置，在任何時(shí)刻都可以使用一個(gè)固定的運(yùn)算，因?yàn)?**這個(gè)地標(biāo)隸屬于地球這個(gè)剛體。但如果使用這個(gè)運(yùn)算去推算我們與月亮的相對(duì)位置，由于月亮與地球不在一個(gè)剛體上，且月亮繞著地球轉(zhuǎn)，所以任何時(shí)刻這個(gè)轉(zhuǎn)換關(guān)系都在變。

了解了位置的表征后，我們?cè)賮?lái)考慮機(jī)器人和自動(dòng)駕駛的定位問(wèn)題，其實(shí)它是要估計(jì)運(yùn)動(dòng)主體（機(jī)器人本身或者車輛）這個(gè)參考幀，相對(duì)于周遭靜止環(huán)境的位姿或者位姿變化， 這個(gè)周遭靜止的環(huán)境，我們可以統(tǒng)稱為世界坐標(biāo)系。移動(dòng)定位問(wèn)題，可以簡(jiǎn)化為跟重力方向垂直的水平面上的2D位姿估計(jì)。 2D坐標(biāo)系的(0,0)點(diǎn), 及x,y軸的朝向其實(shí)可以是任意的，只要基準(zhǔn)定好了，后面的參考不變即可。

對(duì)于自動(dòng)駕駛，有一些細(xì)節(jié)需要補(bǔ)充， 我們熟知的定位（GPS）是經(jīng)緯度坐標(biāo)，如何對(duì)應(yīng)平面笛卡爾坐標(biāo)系呢？

經(jīng)緯度坐標(biāo)可以通過(guò)墨卡托投影系統(tǒng)（Universal Transverse Mercator，UTM）投影到UTM 的一個(gè)區(qū)塊中， 區(qū)塊中再細(xì)的位置可以看成一個(gè)2D平面使用笛卡爾坐標(biāo)進(jìn)行表征， 這樣球面的經(jīng)緯度坐標(biāo)和平面坐標(biāo)是可以轉(zhuǎn)換的。

▍定位需要的傳感器及定位方法

運(yùn)動(dòng)主體想要了解自己的位置，必須借助傳感器。傳感器可以分為以下兩種：

內(nèi)傳感器

1. 原理

內(nèi)傳感器通過(guò)感知自身的運(yùn)動(dòng)變化，計(jì)量累計(jì)位姿變化。

2. 分類

輪子碼盤（又叫輪式里程計(jì)），里程計(jì)通過(guò)事先知道車輪的直徑，計(jì)數(shù)車輪轉(zhuǎn)速，得出速度和位移。 以差分兩輪為例子，根據(jù)兩個(gè)輪子的轉(zhuǎn)圈數(shù)差，可以推算出運(yùn)動(dòng)主體角度的變化。

慣性傳感器（加速度計(jì)、陀螺儀），慣性傳感器可以測(cè)量出線性加速度和角速度，通過(guò)積分可以推算出累計(jì)位移和角度變化。

內(nèi)傳感器的定位， 都依賴于一個(gè)起始位姿，通過(guò)不停積分，在起始位姿基礎(chǔ)上，合并相對(duì)位姿，進(jìn)而推算出新時(shí)刻的位姿。

3. 優(yōu)點(diǎn)

不依賴于外部環(huán)境，不對(duì)外部環(huán)境做先驗(yàn)假設(shè)。

4. 缺點(diǎn)

它推算的是累計(jì)位姿變化，所以絕對(duì)定位需要一個(gè)準(zhǔn)確的起始位姿，起始不對(duì)，后面定位都不準(zhǔn)。既然是累計(jì)變化，如果每一步累計(jì)引入誤差，不管誤差多小，在后續(xù)長(zhǎng)時(shí)間的積累下，都會(huì)是一個(gè)很大的誤差。見下面的點(diǎn)模擬概率分布圖：

外傳感器

1. 原理

外傳感器通過(guò)感知周遭環(huán)境，來(lái)輔助定位自己的位姿。

2. 分類

GPS接收

2D單目攝像頭

雙目攝像頭

深度攝像頭

激光雷達(dá)

……

下面分別描述每種外傳感器的原理和優(yōu)缺點(diǎn)：

GPS接收

優(yōu)點(diǎn)是接收到全局位置錨定的定位信息，不會(huì)擔(dān)心誤差累計(jì)。

缺點(diǎn)有兩個(gè)：

1）獲取位置信息的頻率不會(huì)很快，大概10Hz的樣子。對(duì)于運(yùn)動(dòng)速度很慢的運(yùn)動(dòng)主體，還湊合能用，對(duì)于自動(dòng)駕駛這個(gè)高速行駛的運(yùn)動(dòng)主體，需要更高的頻率獲取位姿。

2）GPS信號(hào)很容易被遮擋， 室內(nèi)定位基本用不上GPS，汽車過(guò)隧道的時(shí)候，會(huì)有相當(dāng)長(zhǎng)的時(shí)間無(wú)GPS信號(hào)。對(duì)室外定位要求比較高的場(chǎng)景，或者室內(nèi)定位場(chǎng)景， GPS無(wú)法單獨(dú)解決定位問(wèn)題。

2D單目攝像頭

2D單目攝像頭可以將3D世界中的物體，投影到2D像素平面。它有一個(gè)特點(diǎn)，像素平面中的物體大小和物體的遠(yuǎn)近可以等比例放大縮小，投射到成像平面是一樣的，也就是說(shuō)在沒(méi)有物體大小先驗(yàn)知識(shí)情況下，是無(wú)法分辨遠(yuǎn)近的。

單目攝像頭定位最大的優(yōu)點(diǎn)是便宜，定位有多種思路，比較典型的有兩種：

1）間接定位

運(yùn)動(dòng)主體可以通過(guò)識(shí)別一些不動(dòng)的物體作為參照，間接進(jìn)行定位。但這個(gè)對(duì)于機(jī)器人來(lái)說(shuō)非常難。因?yàn)橥饨绲沫h(huán)境通過(guò)攝像頭傳入機(jī)器人的都是數(shù)字化的信息，具體就是像素點(diǎn)，數(shù)字化的信息通過(guò)識(shí)別物體的方式進(jìn)行定位，是一件非常吃力的事情。

有一個(gè)變種的方案很方便實(shí)施，就是二維碼方案，機(jī)器人識(shí)別二維碼是非常容易的，通過(guò)知曉二維碼的寬度（只有知曉先驗(yàn)大小，才能判斷遠(yuǎn)近），與不同二維碼所代表的地標(biāo)位置，機(jī)器人可以僅僅通過(guò)單目2D攝像頭方便的推算出自己的絕對(duì)位姿。以前的VR設(shè)備，如HTC VIVE，多采用這種方式實(shí)現(xiàn)定位。這個(gè)方案不方便之處在于要提前部署二維碼。

2）單目SLAM

單目SLAM要做到比較魯棒的定位，需要兩個(gè)階段， 分為前端的視覺(jué)里程計(jì)和后端的回環(huán)機(jī)制。該方案的問(wèn)題包含初始化尺度問(wèn)題和實(shí)際工程實(shí)施中計(jì)算量實(shí)時(shí)性問(wèn)題。

尺度問(wèn)題，是由2D成像理論內(nèi)在特點(diǎn)決定的，因?yàn)闆](méi)有深度信息，大小和遠(yuǎn)近是可以成比例縮放的， 反映到單目SLAM, 在初始化階段，必須運(yùn)動(dòng)主體有平移的動(dòng)作，來(lái)確定一個(gè)尺度基準(zhǔn)，這個(gè)尺度基準(zhǔn)是無(wú)法與測(cè)量單位“m,cm,mm”對(duì)應(yīng)的，只是自己的一個(gè)單位， 后續(xù)的建圖和定位都是基于這個(gè)單位來(lái)進(jìn)行， 建立的地圖和定位理論上可以等比例縮放。

計(jì)算量問(wèn)題，在于SLAM算法本身的復(fù)雜性，勉強(qiáng)在嵌入式設(shè)備上跑，計(jì)算資源基本耗盡。在實(shí)踐中可以從算法并行計(jì)算方面進(jìn)行探索，或者選擇高性能的計(jì)算平臺(tái)。

單目SLAM 分為特征點(diǎn)法和直接法，考慮到對(duì)周遭環(huán)境的魯棒性，一般采用特征點(diǎn)法，就是采用人工設(shè)計(jì)的角點(diǎn)，作為連續(xù)幀進(jìn)行地標(biāo)匹配。角點(diǎn)肯定是稀疏的，所以建圖只是作為定位的輔助地標(biāo)來(lái)做使用， 不能指導(dǎo)避障和導(dǎo)航。

主動(dòng)光深度攝像頭(3D）

這種攝像頭在2D攝像頭的基礎(chǔ)上，增加紅外發(fā)射和接收裝置，在2D RGB像素上增加深度（距離）信息。比較有名的產(chǎn)品有微軟的Kinect，蘋果最新的手機(jī)產(chǎn)品IphoneX會(huì)配備這種攝像頭，提供給用戶VR使用體驗(yàn)和增強(qiáng)人臉識(shí)別FaceId的準(zhǔn)確度。

深度攝像頭在定位方面主要是深度SLAM方案。 相對(duì)于單目SLAM, 因?yàn)槊總€(gè)像素有了深度信息， 這樣尺度問(wèn)題就不存在了，不需要運(yùn)動(dòng)主體做平移運(yùn)動(dòng)來(lái)做初始化動(dòng)作。有了深度信息，理論上建圖是可以做稠密圖，可以做三維建模。

它的缺點(diǎn)也很明顯，除了跟2D一樣算法計(jì)算量偏大，紅外主動(dòng)光非常容易受到其它強(qiáng)光的影響， 這樣在室外基本就退化成一個(gè)2D攝像頭了。 對(duì)于一些透光介質(zhì)，如玻璃，深度信息是無(wú)法得到的。

雙目攝像頭

兩個(gè)攝像頭，光心距離固定。雙目攝像頭通過(guò)視差，可以間接得到兩個(gè)攝像頭共視像素的深度信息。雙目攝像頭比較像人的眼睛, 人可以通過(guò)雙眼，直接估計(jì)出眼前物體的相對(duì)遠(yuǎn)近。對(duì)于計(jì)算機(jī)來(lái)說(shuō)，通過(guò)同一時(shí)刻兩個(gè)攝像頭分別拍的兩幀圖像，根據(jù)視差的幾何關(guān)系，可以算出像素的深度，達(dá)到跟主動(dòng)光深度攝像頭一樣的效果。

相比主動(dòng)光深度攝像頭， 它的優(yōu)點(diǎn)是受環(huán)境光線的影響比較小，可以在室外自動(dòng)駕駛汽車上應(yīng)用。缺點(diǎn)是像素的深度信息不能直接獲取，需要不小的運(yùn)算量通過(guò)視差幾何關(guān)系計(jì)算獲得。

雙目攝像頭本質(zhì)上就是一個(gè)深度攝像頭，只是獲取深度信息的手段不同，所以定位也可以應(yīng)用深度SLAM方案。

激光雷達(dá)

激光雷達(dá)是目前定位選擇的主流傳感器，帶自主導(dǎo)航的室內(nèi)掃地機(jī)的商用產(chǎn)品，一般都會(huì)配備激光雷達(dá)。在自動(dòng)駕駛領(lǐng)域，高精地圖的采集及定位應(yīng)用， 使用的是多線激光雷達(dá)方案。

激光雷達(dá)分為單線和多線， 單線雷達(dá)只能掃描一個(gè)平面的障礙，所以直接出來(lái)的是一個(gè)2D地圖。 多線雷達(dá)（有16線，32線，64線）產(chǎn)品，通過(guò)多個(gè)掃描面的組合，可以給出豐富的環(huán)境3D點(diǎn)云。

激光雷達(dá)定位， 主要是激光SLAM算法，跟視覺(jué)SLAM一樣，也分前端雷達(dá)里程計(jì)和后端回環(huán)檢測(cè)矯正。

激光SLAM對(duì)CPU的消耗，是遠(yuǎn)遠(yuǎn)低于視覺(jué)SLAM的，魯棒性更好，更加穩(wěn)定。以2D激光SLAM為例，它可以在任意時(shí)刻得到某個(gè)特定高度水平面的2D障礙輪廓，所以在做前端里程計(jì)的時(shí)候，連續(xù)兩幀，計(jì)算局部的地圖輪廓匹配，可以使用相對(duì)比較少的計(jì)算量獲取相對(duì)位移。

激光掃描出的點(diǎn)有準(zhǔn)確度很高的深度信息，這樣在做后端回環(huán)優(yōu)化的時(shí)候，不需要優(yōu)化某個(gè)位姿下的觀測(cè)值（掃描的點(diǎn)云）， 而直接優(yōu)化位姿。

對(duì)于視覺(jué)SLAM， 不論是單目SLAM 通過(guò)三角測(cè)量算出的點(diǎn)云深度，還是深度SLAM中獲取到的點(diǎn)深度， 有很大噪聲在里面，所以優(yōu)化要對(duì)觀測(cè)點(diǎn)和位姿一起優(yōu)化調(diào)整。

激光做定位的缺點(diǎn)是受環(huán)境如雨、霧的影響比較大，對(duì)于透明介質(zhì)也無(wú)法得到準(zhǔn)確的深度信息。

▍目前定位應(yīng)用的主要方式

上面我們描述了用于定位的主流傳感器，可以看到單一傳感器在解決定位問(wèn)題上都有自己的優(yōu)缺點(diǎn)。在實(shí)際應(yīng)用中，需要結(jié)合多個(gè)傳感器聯(lián)合解決定位問(wèn)題。 以下針對(duì)幾個(gè)典型場(chǎng)景，描述一下多傳感器融合的情況：

1. 自動(dòng)駕駛 GPS+IMU+里程計(jì)

GPS 給出的全局錨定，可以消除累計(jì)誤差問(wèn)題，不過(guò)它的更新頻率低，并且信號(hào)容易被遮擋。 IMU和輪盤里程計(jì)更新頻率高，不過(guò)有累計(jì)誤差問(wèn)題， 最容易想到的是收到GPS定位，使用GPS位置信息，誤差就是GPS的精度，在下一次收到GPS定位間隔中，使用IMU（角度累加）和里程計(jì)（位移累加）進(jìn)行位姿累加，中間的位姿誤差是初值GPS定位誤差和中間累加誤差的積累。

改進(jìn)的方法是使用非線性卡爾曼濾波，在收到GPS位置信息的時(shí)候，要結(jié)合IMU和里程計(jì)的積累預(yù)測(cè)值和GPS觀測(cè)值，算出一個(gè)誤差收斂的更優(yōu)的位置估算值。

2. 自動(dòng)駕駛 GPS+ 多線雷達(dá)+高精地圖匹配

GPS 給出全局錨定，中間使用雷達(dá)SLAM 前端里程計(jì)做累加，可以配合高精地圖的圖匹配，做類似后端回環(huán)優(yōu)化的方式，將GPS、激光雷達(dá)及已知地圖進(jìn)行融合定位。

3. 自動(dòng)駕駛多對(duì)雙目視覺(jué)攝像頭SLAM方案

這種方案成本低，更加考究的是算法，有很少的自動(dòng)駕駛公司宣稱自己主攻純視覺(jué)方案，現(xiàn)在不是主流。

4. 單線雷達(dá)+IMU+里程計(jì)融合

滿足室內(nèi)定位的要求， 個(gè)人理解可以分為淺融合和深融合。 淺融合使用IMU+里程計(jì)的累加值作為推算雷達(dá)里程計(jì)的初值，在這個(gè)初值基礎(chǔ)上進(jìn)行連續(xù)幀的掃描匹配，會(huì)大大加速匹配速度。 深融合會(huì)結(jié)合IMU和里程計(jì)的值作為約束條件，應(yīng)用到后端回環(huán)約束矯正中。

5. 深度攝像頭+ IMU 融合

目前在手機(jī)的VR應(yīng)用中已經(jīng)初見端倪，如蘋果公司的IphoneX以及Google已經(jīng)發(fā)布一段時(shí)間的Tango項(xiàng)目。 深度視覺(jué)SLAM 與 IMU 進(jìn)行深淺融合，達(dá)到一個(gè)比較不錯(cuò)的VR體驗(yàn)。

以上組合只是市面上能看到的一些產(chǎn)品采用的定位手段，當(dāng)然還可以結(jié)合單目SLAM, 雙目SLAＭ與一些內(nèi)傳感器，進(jìn)行隨意組合。融合方案的定位精度會(huì)優(yōu)于單一傳感器，一個(gè)傳感器在某種環(huán)境失效，補(bǔ)充傳感器能頂上。