談到移動機(jī)器人，大家第一印象可能是服務(wù)機(jī)器人，實際上無人駕駛汽車、可自主飛行的無人機(jī)等等都屬于移動機(jī)器人范疇。它們能和人一樣能夠在特定的環(huán)境下自由行走／飛行，都依賴于各自的定位導(dǎo)航、路徑規(guī)劃以及避障等功能，而視覺算法則是實現(xiàn)這些功能關(guān)鍵技術(shù)。

如果對移動機(jī)器人視覺算法進(jìn)行拆解，你就會發(fā)現(xiàn)獲取物體深度信息、定位導(dǎo)航以及壁障等都是基于不同的視覺算法，本文就帶大家聊一聊幾種不同但又必不可少的視覺算法組成。

本文作者陳子沖，系Segway Robot架構(gòu)師和算法負(fù)責(zé)人。

Q：實現(xiàn)定位導(dǎo)航、路徑規(guī)劃以及避障，那么這些過程中需要哪些算法的支持？

談起移動機(jī)器人，很多人想到的需求可能是這樣的：“嘿，你能不能去那邊幫我拿一杯熱拿鐵過來?！边@個聽上去對普通人很簡單的任務(wù)，在機(jī)器人的世界里，卻充滿了各種挑戰(zhàn)。為了完成這個任務(wù)，機(jī)器人首先需要載入周圍環(huán)境的地圖，精確定位自己在地圖中的位置，然后根據(jù)地圖進(jìn)行路徑規(guī)劃控制自己完成移動。

而在移動的過程中，機(jī)器人還需要根據(jù)現(xiàn)場環(huán)境的三維深度信息，實時的躲避障礙物直至到達(dá)最終目標(biāo)點。在這一連串機(jī)器人的思考過程中，可以分解為如下幾部分的視覺算法：

1．深度信息提取

2．視覺導(dǎo)航

3．視覺避障

后面我們會詳細(xì)說這些算法，而這些算法的基礎(chǔ)，是機(jī)器人腦袋上的視覺傳感器。

視覺算法的基礎(chǔ)：傳感器

Q：智能手機(jī)上的攝像頭可以作為機(jī)器人的眼睛嗎？

所有視覺算法的基礎(chǔ)說到底來自于機(jī)器人腦袋上的視覺傳感器，就好比人的眼睛和夜間視力非常好的動物相比，表現(xiàn)出來的感知能力是完全不同的。同樣的，一個眼睛的動物對世界的感知能力也要差于兩個眼睛的動物。每個人手中的智能手機(jī)攝像頭其實就可以作為機(jī)器人的眼睛，當(dāng)下非常流行的Pokeman Go游戲就使用了計算機(jī)視覺技術(shù)來達(dá)成AR的效果。

像上圖畫的那樣，一個智能手機(jī)中攝像頭模組，其內(nèi)部包含如下幾個重要的組件：鏡頭，IR filter，CMOS sensor。其中鏡頭一般由數(shù)片鏡片組成，經(jīng)過復(fù)雜的光學(xué)設(shè)計，現(xiàn)在可以用廉價的樹脂材料，做出成像質(zhì)量非常好的手機(jī)攝像頭。

CMOS sensor上面會覆蓋著叫做Bayer三色濾光陣列的濾色片。每個不同顏色的濾光片，可以通過特定的光波波長，對應(yīng)CMOS感光器件上就可以在不同位置分別獲得不同顏色的光強(qiáng)了。如果CMOS傳感器的分辨率是4000x3000，為了得到同樣分辨率的RGB彩色圖像，就需要用一種叫做demosaicing的計算攝像算法，從2綠1藍(lán)1紅的2x2網(wǎng)格中解算出2x2的RGB信息。

一般的CMOS感光特性除了選擇紅綠藍(lán)三色之外，對于紅外光是透明的。因此在光路中加上IR濾光片，是為了去除太陽光線中紅外光對CMOS的干擾。加上濾光片后，通常圖像的對比度會得到顯著的提升。

Q：計算機(jī)視覺中還會用到什么傳感器？

除了RGB相機(jī)，計算機(jī)視覺中常用的還有其他種類的特殊相機(jī)。例如有一種相機(jī)的濾光片是只允許通過紅外光波段的。因為人眼通常是看不見紅外光的，所以可以在相機(jī)附近加上主動紅外光源，用于測距等應(yīng)用。

另外，大部分我們用到的camera都是以rolling shutter的形式實現(xiàn)電子曝光的，像圖中左側(cè)那樣，為了減少電子器件的成本，曝光通常是一行一行分別進(jìn)行，這樣勢必造成物體快速移動時，相機(jī)采集到的圖像會發(fā)生形變。為了避免這種形變對基于立體幾何進(jìn)行計算的視覺算法的影響（例如VSLAM），選用global shutter的相機(jī)就顯得特別重要了。

深度相機(jī)是另一大類視覺算法中需要的傳感器，可以分成如下幾類：

1．TOF傳感器（例如Kinect 2代），類似昆蟲復(fù)眼。成本高，室外可以使用。

2．結(jié)構(gòu)光傳感器（例如Kinect 1代），三角定位原理，成本中，室外不能用。

3．雙目視覺（例如Intel Realsense R200），主動照明或被動照明，IR或可見光皆可。成本低，室外可以使用。

算法一：深度信息提取

Q：深度相機(jī)如何識別物體的深度信息的呢？

簡而言之，其原理就是使用兩個平行的相機(jī)，對空間中的每個點三角定位。通過匹配左右兩個相機(jī)中成像點的位置，來計算對應(yīng)三維點在空間中的距離。學(xué)術(shù)界對雙目匹配恢復(fù)深度圖研究有很長的歷史，在NASA火星車上就開始采用這個技術(shù)。但是其真正在消費(fèi)電子品市場得到廣泛應(yīng)用還是從微軟的Kinect體感傳感器開始。

Kinect傳感器背后使用了以色列Primesense公司授權(quán)的結(jié)構(gòu)光技術(shù)（如今已被Apple收購）。其原理是避開雙目匹配中復(fù)雜的算法設(shè)計，轉(zhuǎn)而將一個攝像頭更換成向外主動投射復(fù)雜光斑的紅外投影儀，而另一個平行位置的相機(jī)也變成了紅外相機(jī)，可以清楚的看到投影儀投射的所有光斑。因為人眼看不到紅外光斑，而且紋理非常復(fù)雜，這就非常有利于雙目匹配算法，可以用非常簡潔的算法，識別出深度信息。

盡管Kinect的內(nèi)在原理官方并沒有給出解釋，在近年來一篇Kinect Unleashed的文章中，作者向公眾hack了這個系統(tǒng)的工作原理：

首先，紅外圖像在基線方向上采樣8倍，這樣可以保證在做雙目匹配后實現(xiàn)3bit的亞像素精度。然后，對圖像做sobel濾波，使得圖像的匹配精度提高。而后，圖像與預(yù)存的投影光斑模板圖像進(jìn)行SAD block matching。該算法的計算復(fù)雜度小，適合硬化和并行。最后，經(jīng)過簡單的圖像后處理，下采樣到原始分辨率，得到最終的深度圖。

我們可以看到，隨著2009年Kinect設(shè)備在消費(fèi)機(jī)市場的爆發(fā)（發(fā)售頭10天100萬臺），逐漸開始催生了類似技術(shù)變種在移動端設(shè)備的研發(fā)熱潮。從2013年至今，隨著計算能力的提升和算法的進(jìn)步，硬件成本更低的主動／被動雙目深度相機(jī)開始在移動手機(jī)上涌現(xiàn)。

過去認(rèn)為很難實時運(yùn)行的雙目匹配算法，即使在沒有主動結(jié)構(gòu)光輔助的情況下，也表現(xiàn)出非常優(yōu)異的3D成像質(zhì)量。Segway robot采用了主動／被動可切換的雙目深度視覺系統(tǒng)。如下圖所示，左側(cè)三個傳感器分別為，左紅外相機(jī)，紅外pattern投影，右紅外相機(jī)。在室內(nèi)工作時，因為紅外光源不足，紅外投影打開，輔助雙目匹配算法。在室外工作時，紅外光源充足，紅外投影關(guān)閉，雙目匹配算法可以直接運(yùn)行。綜合看，此系統(tǒng)在室內(nèi)外都表現(xiàn)出優(yōu)異的深度傳感能力。

算法二：定位導(dǎo)航

Q：視覺處理后，機(jī)器人是如何實現(xiàn)導(dǎo)航的？

機(jī)器人導(dǎo)航本身是一個比較復(fù)雜的系統(tǒng)。其中涉及到的技術(shù)會有如下列表：

視覺里程計 VO

建圖，利用VO和深度圖

重定位，從已知地圖中識別當(dāng)前的位置

閉環(huán)檢測·，消除VO的閉環(huán)誤差

全局導(dǎo)航

視覺避障

Scene tagging，識別房間中物體加上tag

機(jī)器人開機(jī)，視覺里程計就會開始工作，記錄從開機(jī)位置起的6DOF定位信息。在機(jī)器人運(yùn)動過程中，mapping算法開始構(gòu)建機(jī)器人看到的世界，將空間中豐富的特征點信息，二維的地圖信息記錄到機(jī)器人map中。

當(dāng)機(jī)器人運(yùn)動過程中因為遮擋、斷電等原因丟失了自身的坐標(biāo)，重定位算法就需要從已知地圖中定位到機(jī)器人當(dāng)前的位置估計。另外，當(dāng)機(jī)器人運(yùn)動中回到了地圖中曾經(jīng)出現(xiàn)過的位置，往往視覺里程計的偏差會導(dǎo)致軌跡并沒有完全閉合，這就需要閉環(huán)算法檢測和糾正這個錯誤。

有了全局地圖之后，機(jī)器人就可以給定一些目標(biāo)點指令，做全局的自主導(dǎo)航了。在現(xiàn)實中，因為環(huán)境是不停變化的，全局地圖并不能完全反映導(dǎo)航時的障礙物狀況，因此需要凌駕于全局導(dǎo)航之上的視覺避障算法進(jìn)行實時的運(yùn)動調(diào)整。

最后，一個自動的導(dǎo)航系統(tǒng)還需要機(jī)器人自動識別和理解空間中的不同物體的信息、位置、高度和大小。這些tag信息疊加在地圖上，機(jī)器人就可以從語義上理解自己所處的環(huán)境，而用戶也可以從更高層次下達(dá)一些指令。

Q：視覺VSLAM在機(jī)器人上的實現(xiàn)有哪些難點？

視覺VSLAM是一個集合了視覺里程計，建圖，和重定位的算法系統(tǒng)。近年來發(fā)展很快。基于特征的視覺SLAM算法從經(jīng)典的PTAM算法開端，目前以O(shè)RB－SLAM為代表的算法已經(jīng)可以在PC上達(dá)到實時運(yùn)行。下面是一個ORBSLAM的框圖：

從名字可見，其使用ORB作為圖像特征提取工具，并且在后續(xù)的建圖及重定位中均使用了同一份特征點信息。相對于傳統(tǒng)的SIFT和SURF特征提取算法，其效率高出很多。

ORB－SLAM包含三個并行的線程，即跟蹤，建圖和閉環(huán)。其中跟蹤線程運(yùn)行在前端，保證實時運(yùn)行，建圖和閉環(huán)線程運(yùn)行在后端，速度不需要實時，但是與跟蹤線程共享同一份地圖數(shù)據(jù)，可以在線修正使得地圖數(shù)據(jù)精度和跟蹤精度更高。下圖是ORB－SLAM地圖的主要數(shù)據(jù)結(jié)構(gòu)，

點云和關(guān)鍵幀。兩者之間通過圖像上2D特征點與空間中的點云建立映射關(guān)系，同時還維護(hù)了關(guān)鍵幀之間的covisibility graph關(guān)系。通過這些數(shù)據(jù)關(guān)聯(lián)，用優(yōu)化方法來維護(hù)整個地圖。

ORB－SLAM在機(jī)器人上應(yīng)用仍然存在如下難點：

1．計算量過大，在4核處理器上通常會占去60％左右CPU資源。

2．在機(jī)器人運(yùn)動過快時會出現(xiàn)跟丟不可復(fù)原的情況。

3．單目SLAM存在尺度不確定的問題。在機(jī)器人快速旋轉(zhuǎn)時，此問題尤其明顯，很快會出現(xiàn)閉環(huán)誤差過大無法糾正的情況。

針對尺度問題，有兩種方法解決：增加一個攝像頭形成雙目SLAM系統(tǒng)，或者增加一個IMU形成松耦合／緊耦合的視覺慣導(dǎo)定位系統(tǒng)。這里簡單介紹松耦合的視覺慣導(dǎo)定位系統(tǒng)。一般把VSLAM當(dāng)成一個黑盒子，將其的輸出作為觀測量放到一個基于IMU的EKF系統(tǒng)中，EKF最終fuse的輸出即是系統(tǒng)的輸出。

考慮到camera數(shù)據(jù)和IMU數(shù)據(jù)通常是不同步的，因此通過硬件時間戳，需要判斷圖像數(shù)據(jù)對應(yīng)的時間戳與IMU時間戳的關(guān)系。在EKF propagate步驟，更高幀率的IMU數(shù)據(jù)不停的更新EKF的狀態(tài)。在camera數(shù)據(jù)到來時，觸發(fā)EKF update步驟，根據(jù)EKF建模方程來更新狀態(tài)變量、協(xié)方差矩陣，并且重新更新所有晚于camera數(shù)據(jù)的IMU數(shù)據(jù)對應(yīng)的狀態(tài)變量。

Segway Robot采用了業(yè)界領(lǐng)先的視覺慣導(dǎo)定位系統(tǒng)，下面是一個在樓道里面運(yùn)行一圈，回到原點之后的效果圖，具體有如下優(yōu)勢：

1．在大尺度下可以保證非常小的閉環(huán)誤差

2．實時運(yùn)行，需求CPU資源小

3．允許快速旋轉(zhuǎn)等情形，不會跟丟

算法三：避障

Q：視覺避障的算法原理是怎樣的？

導(dǎo)航解決的問題是引導(dǎo)機(jī)器人接近目標(biāo)。當(dāng)機(jī)器人沒有地圖的時候，接近目標(biāo)的方法稱為視覺避障技術(shù)。避障算法解決的問題是根據(jù)視覺傳感器的數(shù)據(jù)，對靜態(tài)障礙物、動態(tài)障礙物實現(xiàn)躲避，但仍維持向目標(biāo)方向運(yùn)動，實時自主導(dǎo)航。

避障算法有很多，然而這些方法都有嚴(yán)格的假設(shè)，假設(shè)障礙物為圓形或假設(shè)機(jī)器人為圓形，假設(shè)機(jī)器人可以任意方向運(yùn)動，s或假設(shè)它只能走圓弧路徑。然而實際應(yīng)用上，機(jī)器人很難達(dá)到條件。比如VFF算法， 該算法假設(shè)機(jī)器人為點，而且可以任意方向運(yùn)動。VFH＋假設(shè)機(jī)器人為圓形，通過圓形膨脹障礙物，在考慮運(yùn)動學(xué)問題時僅僅假設(shè)機(jī)器人以圓弧路徑運(yùn)動。DWA也假設(shè)機(jī)器人為圓形，在考慮運(yùn)動學(xué)問題時只模擬了前向圓弧運(yùn)動時的情況。

相對而言，我們不限制機(jī)器人的形狀，考慮運(yùn)動學(xué)問題時，模擬多種運(yùn)動模型，而不限于圓弧運(yùn)動，因為這樣可以為機(jī)器人找到更佳避開障礙物的行為。

這張圖顯示了使用不同運(yùn)動學(xué)模型導(dǎo)致不同的避障結(jié)果。左圖表示使用圓弧模型時模擬的路徑，右圖表示使用另一種路徑模型模擬的路徑。在這種狹小環(huán)境，此方法可以提前預(yù)測多個方向的障礙物情況，選擇合適的模型可以幫助找到更合適的運(yùn)動方向躲避障礙物。

和目前常用的避障算法之間存在的差異在于，它將運(yùn)動學(xué)模型抽象化到周圍環(huán)境地圖中，然后就可以使用任何常用的避障算法，這樣就解耦了運(yùn)動學(xué)模型與算法的捆綁，而且任何要求嚴(yán)格的避障算法都能加入進(jìn)來。Segway Robot的避障系統(tǒng)，綜合了深度傳感器，超聲波，IMU等sensor。在復(fù)雜的環(huán)境中，可以自如躲避障礙物。

這張圖是我們的避障系統(tǒng)的一個截圖，可以看到深度圖和2維的避障地圖。最下面紅色的指針就代表了每時每刻避障的決策。

精彩問答

Q：為什么選用ir相機(jī)而不是傳統(tǒng)的rgb相機(jī)呢？ir相機(jī)相對來講的優(yōu)勢在哪里？

A：ir相機(jī)可以看到人眼看不到的物體，比如深度相機(jī)需要在室內(nèi)投射紅外紋理，幫助深度識別。人眼看不到，但ir相機(jī)可以看。

Q：現(xiàn)在機(jī)器人導(dǎo)航是否主要是slam技術(shù)，還有沒其他導(dǎo)航技術(shù)？主要流行的slam技術(shù)有哪些？用于無人駕駛和無人機(jī)的視覺導(dǎo)航技術(shù)有哪些異同？

A：slam技術(shù)是導(dǎo)航中的一個基礎(chǔ)模塊，種類很多，有單目，雙目，depth，imu＋視覺等傳感器為基礎(chǔ)的算法。雙目相機(jī)可以很好的適應(yīng)室內(nèi)和室外的環(huán)境。他的體積其實非常小，segway robot使用的camera長度在10cm左右

Q：現(xiàn)在有無用于機(jī)器人導(dǎo)航的導(dǎo)航地圖存在，類似車載導(dǎo)航地圖？用于機(jī)器人導(dǎo)航的地圖數(shù)據(jù)有哪些？

A：現(xiàn)在還沒有這樣的機(jī)器人導(dǎo)航地圖存在，但是是研發(fā)熱點。比如tesla和mobileye的地圖之爭。