定位問題的引入

我們所熟知的機器人，可以大致分為以下四類：
? ? ? ? 1. 操作機器人： 模仿人的手和手臂的動作，特點是有多個關(guān)節(jié)自由度。
? ? ? ? 2. 移動機器人：通過移動，空間變換完成特定任務(wù)（運輸，覆蓋，上下料，到達某地）。
? ? ? ? 3. 信息機器人：計算機系統(tǒng)的智能行為裝置，人工智能。
? ? ? ? 4. 人機結(jié)合： 機器人與真人之間的控制反饋閉合，如假肢機器人。
這四種類型的機器人可以做組合，如有智能行為的移動機器人，機器人可以移動并且做一些操作行為。 前兩個類型的機器人，是發(fā)展比較成熟的，已經(jīng)大量應(yīng)用在工業(yè)領(lǐng)域，商業(yè)和家庭領(lǐng)域在近些年應(yīng)用比例也在增長。其中移動機器人是我們目前關(guān)心的重點，運動控制平臺與自動駕駛都能歸結(jié)為移動機器人，移動機器人包含無人機，水下機器人,輪式機器人，履帶式機器人，仿生足大狗等。 移動機器人關(guān)心的問題包含：
? ? ? ? 1. ?where am I (定位問題）
? ? ? ? 2. ?where am I going(導(dǎo)航問題，調(diào)度問題）
? ? ? ? 3. ?How do I get there（避障問題， 動作規(guī)劃）
移動機器人的核心問題就是定位問題， 上面列的后面兩個問題，都是以定位問題為前提，才能達成。

定位的基本概念

了解定位，我們首先要先了解位置。位置這個術(shù)語其實不是很準確，應(yīng)該叫做位姿，包含不僅位置，還有目標體的朝向（姿態(tài)）。我們習(xí)慣于用笛卡爾坐標系來表征位姿。 2D平面的位姿，有三個自由度(x,y,rotation)，x,y 是2D平面的坐標位置，rotation 是偏向角。3D世界有六個自由度（x,y,z, yaw,pitch,roll)。x,y,z 是3D立體坐標系中的位置坐標， yaw(航向角），pitch(俯仰角）,roll(傾斜角）分別代表目標剛體繞z,y,x軸按照順序旋轉(zhuǎn)后的朝向姿態(tài)。表示旋轉(zhuǎn)有很多種方法，這里介紹的yaw,pitch,roll叫做歐拉角表示法，是比較容易理解的表示法，這里要注意的是旋轉(zhuǎn)的順序及之后是按照新位姿的坐標軸進行旋轉(zhuǎn)，這是常規(guī)的表征方法，不同的順序，及坐標軸的選取標準不同，出來的位姿將是不同的結(jié)果。具體到自動駕駛和運動控制平臺，都是在一個平面上進行移動，雖然中間有顛簸，但是我們關(guān)注的是在水平面上的位姿，上下的顛簸引起的位姿變更，可以投影到水平面上，這樣定位問題其實是2D位姿的估計問題。與之相對的如無人機就是3D定位問題。

理解定位，必須理解的另外一個概念就是參考基準，通俗講就是相對誰的位置和姿態(tài)，我們舉生活中的例子，做公交汽車，我們相對公交車位置幾乎是不變的，相對于馬路上的某個站牌，是一直在變化的。更加嚴謹?shù)亩x是參考幀（frame)， 這個參考幀具化成視覺效果，是一個坐標系，如下圖，遵守右手法則，規(guī)定了在空間的零點位置和三叉戟坐標軸方向。

在這里有一個非常有意思的地方，可以細細琢磨，定位是研究參考幀與參考幀的相對位置關(guān)系。當我們向別人描述我在哪里的時候，其實是描述，我作為主體的參考幀相對于某個地標（如天安門），甚至地球這個主體參考幀的相對位姿描述，放到太陽系，就不對了，雖然我們描述位置的時候，常常是忽略掉這個地球參考幀的，那是建立在大家都有這個地球參考幀的共識下，去討論位置。 這里還有一個引申的概念就是剛體，組成這個剛體的所有參考幀的互相之間的相對位姿在任何時刻是不變的。 我們與天安門的相對位置，在任何時刻，都可以使用一個固定的運算，推算出我們離美國自由女神像的相對位置，這些地標隸屬于地球這個剛體的，同樣去與推算月亮位置，那么時刻這個轉(zhuǎn)換關(guān)系都在變，地球和月球不在一個剛體上。

了解了位置的表征后，我們再來考慮運動平臺和自動駕駛的定位問題，其實是要估計的是運動主體（運動平臺本身或者車輛）這個參考幀，相對于周遭靜止環(huán)境（其實這個靜止大多數(shù)情況是指的隸屬于地球這個剛體的相對位置靜止）的位姿或者位姿變化， 這個周遭靜止的環(huán)境，我們可以統(tǒng)稱為世界坐標系，移動定位問題，可以簡化為跟重力方向垂直的水平面上的2D位姿估計。 2D坐標系的(0,0)點, 及x,y軸的朝向其實可以是任意的，只要基準定好了，后面的參考不變即可。 對于自動駕駛，有一些細節(jié)需要補充， 我們熟知的定位（GPS）是經(jīng)緯度坐標，如何對應(yīng)平面笛卡爾坐標系呢？ 經(jīng)緯度坐標可以通過墨卡托投影系統(tǒng)（Universal Transverse Mercator，UTM）投影到UTM 的一個區(qū)塊中， 區(qū)塊中再細的位置的可以看成一個2D平面使用笛卡爾坐標進行表征。 這樣球面的經(jīng)緯度坐標和平面坐標是可以轉(zhuǎn)換的。

定位需要的傳感器及相應(yīng)定位方法

運動主體想要了解自己的位置，必須借助傳感器。傳感器可以分為兩種， 內(nèi)傳感器和外傳感器。

內(nèi)傳感器通過感知自身的運動變化，計量累計位姿變化。內(nèi)傳感器包含輪子碼盤（又叫輪式里程計），慣性傳感器（加速度計，陀螺儀）。里程計通過事先知道車輪的直徑，計數(shù)車輪轉(zhuǎn)速，得出速度和位移。 以差分兩輪為例子，根據(jù)兩個輪子的轉(zhuǎn)圈數(shù)差，可以推算出運動主體角度的變化。慣性傳感器可以測量出線性加速度和角速度，通過積分可以推算出累計位移和角度變化。內(nèi)傳感器的定位， 都依賴于一個起始位姿，通過不停積分，在起始位姿基礎(chǔ)上，合并相對位姿，進而推算出新時刻的位姿。內(nèi)傳感器的優(yōu)點是不依賴于外部環(huán)境，不對外部環(huán)境做先驗假設(shè)。缺點也非常明顯， 首先它推算的是累計位姿變化，所以絕對定位需要一個準確的起始位姿，起始不對，后面定位都不準。既然是累計變化，如果每一步累計引入誤差，不管誤差多小，在后續(xù)足夠長的積累下，都會是一個很大的誤差。見下圖（點模擬概率分布）：

外傳感器，通過感知周遭環(huán)境，來輔助定位自己的位姿。外傳感器包含GPS接收，2D單目攝像頭，雙目攝像頭， 深度攝像頭， 激光雷達等。

GPS接收的優(yōu)點是接收到全局位置錨定的定位信息，不會擔(dān)心誤差累計， 缺點有兩個，第一是它獲取位置信息的頻率不會很快，大概10hz的樣子。對于運動速度很慢的運動主體，還湊合能用，對于自動駕駛這個高速行駛的運動主體，需要更高的頻率位姿獲取。第二個缺點是GPS信號很容易被遮擋， 室內(nèi)定位基本用不上GPS，汽車過隧道的時候，會有相當?shù)臅r間無GPS信號。對室外對定位要求比較高的場景，或者室內(nèi)定位場景， GPS無法單獨解決定位問題。

2D單目攝像頭，可以將3D世界中的物體，投影到2D像素平面。它有一個特點，像素平面中的物體大小和物體的遠近可以等比例放大縮小，投射到成像平面是一樣的，這樣也就是說在沒有物體大小先驗知識情況下，是無法分辨遠近的。

單目攝像頭定位最大的優(yōu)點是便宜。定位有多種思路，比較典型的以下兩種：
?????? 1.? 人如何通過眼睛知道自己在哪？ 人是通過識別一些靜止的物體（我們統(tǒng)一稱為地標），來大致估計自己的位置。如在家中， 我們通過看到門，屋子里的擺設(shè)，從這個屋子可以輕松的去到那屋。 在室外，我們通過樓房，車站，樹木等靜止不變的地標，來確定方位。借鑒這個思路，如果運動主體可以通過識別一些不動的物體作為參照，就可以間接進行定位了。不幸的是，這個對于機器人來說，非常難。外界的環(huán)境通過攝像頭傳入機器人的都是數(shù)字化的信息，具體就是像素點，數(shù)字化的信息通過識別物體的方式進行定位，是一件非常吃力的事情。有一個變種的方案，是很方便實施的，就是二維碼方案，機器人識別二維碼，是非常容易的，通過知曉二維碼的寬度（只有知曉先驗大小，才能判斷遠近），不同二維碼所代表的地標位置，機器人可以僅僅通過單目2D攝像頭方便的推算出自己的絕對位姿。以前的VR設(shè)備，如HTC VIVE，多采用這種方式實現(xiàn)定位。這個方案不方便之處在于要提前部署二維碼。
??????? 2. 單目SLAM(simultaneous localization and mapping)， 做到比較魯棒的定位，需要兩個階段， 分為前端的視覺里程計和后端的回環(huán)機制。單目SLAM的問題包含初始化尺度問題和實際工程實施中計算量實時性問題。 尺度問題是2D成像理論內(nèi)在特點決定的，因為沒有深度信息，大小和遠近是可以成比例縮放的， 反映到單目SLAM, 在初始化階段，必須運動主體有平移的動作，來確定一個尺度基準，這個尺度基準是無法與測量單位m,cm,mm對應(yīng)的，只是自己的一個單位， 后續(xù)的建圖和定位都是基于這個單位來進行， 建立的地圖和定位理論上可以等比例縮放。計算量問題，在于SLAM算法本身的復(fù)雜性，勉強在嵌入式設(shè)備上跑，計算資源基本耗盡。在實踐中可以從算法并行計算方面進行探索，或者選擇高性能的計算平臺。單目SLAM 分為特征點法和直接法，考慮到對周遭環(huán)境（光線變化）魯棒性，一般采用特征點法，就是采用人工設(shè)計的角點，作為連續(xù)幀進行匹配的地標。角點肯定是稀疏的，所以建圖只是作為定位的輔助地標來做使用， 不能指導(dǎo)避障和導(dǎo)航。

主動光深度攝像頭(3D）攝像頭，這種攝像頭在2D攝像頭的基礎(chǔ)上，增加紅外發(fā)射和接收裝置。這種主動光裝置目前分結(jié)構(gòu)光三角測距或者TOF(Time of flight)，可以在2D RGB像素上增加深度信息。比較有名的產(chǎn)品有微軟的Kinect。 蘋果最新的手機產(chǎn)品IphoneX 會配備這種攝像頭，提供給用戶VR使用體驗和增強人臉識別FaceId的準確度。深度攝像頭在定位方面主要是深度SLAM方案。 相對于單目SLAM, 因為每個像素有了深度信息， 這樣尺度問題就不存在了， 不需要運動主體做平移運動，來做初始化動作。有了深度信息，理論上建圖是可以做稠密圖，可以做三維建模。它的缺點也很明顯，除了跟2D一樣的算法計算量偏大，紅外主動光非常容易收到其他強光的影響， 這樣在室外，基本就是退化成一個2D攝像頭了。 對于一些透光介質(zhì)，如玻璃，深度信息是無法得到的。

雙目攝像頭，兩個攝像頭，光心距離固定。雙目攝像頭通過視差，可以間接得到兩個攝像頭共視像素的深度信息。雙目攝像頭比較像人的眼睛, 人可以通過雙眼，直接就可以估計出眼前物體相對遠近。對于計算機來說，通過同一時刻兩個攝像頭分別拍的兩幀圖像，根據(jù)視差的幾何關(guān)系，可以算出像素的深度，達到跟主動光深度攝像頭一樣的效果。 相比主動光深度攝像頭， 它的優(yōu)點是受環(huán)境光線的影響比較小，可以在室外自動駕駛汽車上應(yīng)用。缺點也非常明顯，像素的深度信息不是直接就可以獲取的，需要不小的運算量通過視差幾何關(guān)系計算獲得。雙目攝像頭本質(zhì)就是一個深度攝像頭，只是獲取深度信息手段不同，所以定位也可以應(yīng)用深度SLAM方案。

激光雷達是目前定位主流選擇，室內(nèi)掃地機的商用產(chǎn)品，帶自主導(dǎo)航的，一般都會配備激光雷達。而在自動駕駛領(lǐng)域，高精地圖的采集，及定位應(yīng)用， 是使用的多線激光雷達方案。激光雷達分為單線和多線， 單線雷達只能掃描一個平面的障礙，所以直接出來的是一個2D地圖。 多線雷達產(chǎn)品（有16線，32線，64線）產(chǎn)品，通過多個掃描面的組合，可以給出豐富的環(huán)境3D 點云。 激光雷達定位， 主要是激光SLAM算法，跟視覺SLAM一樣，也分前端雷達里程計和后端回環(huán)檢測矯正。激光SLAM 對CPU的消耗，是遠遠低于視覺SLAM的，魯棒性更好，更加穩(wěn)定。以2D激光SLAM為例，它可以在任意時刻得到某個特定高度水平面的2D障礙輪廓，所以在做前端里程計的時候，連續(xù)兩幀，計算局部的地圖的輪廓匹配，可以使用相對比較少的計算量獲取相對位移。 激光掃出的點，有很高準確度的深度信息，這樣在做后端回環(huán)優(yōu)化的時候，不需要優(yōu)化某個位姿下的觀測值（掃描的點云）， 而直接優(yōu)化位姿。 對于視覺SLAM， 不論是單目SLAM 通過三角測量算出的點云深度，還是深度SLAM中獲取到的點深度， 有很大噪聲在里面，所以優(yōu)化是觀測點和位姿都要一起優(yōu)化調(diào)整的。 激光做定位的缺點是受環(huán)境如雨，霧影響比較大，對于透明介質(zhì)也無法得到準確深度信息。

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

上一章節(jié)，我們描述了用于定位的主流傳感器，及對使用這個傳感器進行定位的主流方法進行了簡單介紹。可以看到單一傳感器在解決定位問題上，都有自己的缺點和優(yōu)點， 在實際應(yīng)用中，都是要結(jié)合多個傳感器，聯(lián)合解決定位問題， 以下針對幾個典型場景，描述一下傳感器融合情況。

自動駕駛 GPS+IMU+里程計場景。GPS 給出的全局錨定，可以消除累計誤差問題，不過它的更新頻率低，并且信號容易被遮擋。 IMU ，輪盤里程計更新頻率高，不過有累計誤差問題， 最容易想到的是收到GPS定位，使用GPS位置信息，誤差就是GPS的精度，在下一次收到GPS定位間隔中，使用IMU（角度累加）和里程計（位移累加）進行位姿累加，中間的位姿誤差是初值GPS定位誤差和中間累加誤差的積累。 改進的方法是使用非線性卡爾曼濾波，在收到GPS位置信息的時候，要結(jié)合IMU和里程計的積累預(yù)測值，和GPS觀測值，算出一個誤差收斂的，更優(yōu)的位置估算值。

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

自動駕駛多對雙目視覺攝像頭SLAM方案，這種方案是低成本，更加考究的是算法，有很少的自動駕駛公司宣稱自己主攻純視覺方案，現(xiàn)在不是主流。

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

深度攝像頭+ IMU 融合， 目前在手機的VR應(yīng)用中已經(jīng)初見端倪，如蘋果公司IphoneX及google 已經(jīng)發(fā)布一段時期的Tango項目。 深度視覺SLAM 與 IMU 進行深淺融合，達到一個比較不錯的VR體驗。

以上的組合只是能看到的一些產(chǎn)品的定位手段評估，當然可以結(jié)合單目SLAM, 雙目SLAＭ與一些內(nèi)傳感器，進行隨意組合。　融合是解決兩個問題：定位精度會優(yōu)于單一傳感器，一個傳感器在某種環(huán)境失效，補充傳感器能頂上。

總結(jié)

以上，通過對于定位概念和方法的簡單介紹，讓普通讀者了解定位要解決的問題及常用手段。里面涉及的算法，只做了一些簡單描述和優(yōu)缺點總結(jié)，留待后續(xù)可以展開描述。