背景介紹： 機器人的自主定位導航

正如圖中所示，機器人自主定位導航技術中包括：定位和地圖建立（SLAM）與路徑規劃和運動控制兩個部分，而SLAM本身只是完成機器人的定位和地圖建立，二者有所區別。

首先明確一下自主導航的兩大解決方案：

1.傳統方案（稍微成熟一點）：SLAM+路徑規劃+運動控制；

2.深度學習（還不完善）：
這一兩年，深度學習成為人工智慧的代名詞。在這個行業，前幾年還是以比較傳統的概率學，或者控制論的方式進行機器人自主定位導航。但這裡展現的是完全通過深度學習，直接通過攝像頭資料作為訊號輸入，再通過神經網路直接產生出機器人的控制訊號。其中像 SLAM 的過程、路徑規劃的過程，完全通過學習的方式就可以進行實現。

SLAM

SLAM是同步定位與地圖構建(Simultaneous Localization And Mapping)的縮寫，最早由Hugh Durrant-Whyte 和 John J.Leonard提出。

其實，SLAM更像是一個概念而不是一個演算法，它本身包含許多步驟，其中的每一個步驟均可以使用不同的演算法實現。主要用於解決移動機器人在未知環境中執行時即時定位與地圖構建的問題。

當你身處異地，怎麼準確找到想去的地方？在戶外迷路時，怎麼找到回家的路？沒錯，我們有導航軟體和戶外地圖。

1.選擇地圖

和人類繪製地圖一樣，機器人描述環境、認識環境的過程主要就是依靠地圖。它利用環境地圖來描述其當前環境資訊，並隨著使用的演算法與感測器差異采用不同的地圖描述形式。

機器人學中地圖的表示方法有四種：柵格地圖、特徵地圖、直接表徵法以及拓撲地圖。

（1）.柵格地圖

機器人對環境地圖的描述的方式最常見的為柵格地圖(Grid map)或者稱為Occupancy Map。柵格地圖就是把環境劃分成一系列柵格，其中每一柵格給定一個可能值，表示該柵格被佔據的概率。

這種地圖看起來和人們所認知的地圖沒什麼區別，它最早由NASA的Alberto Elfes在1989年提出，在火星探測車上就用到過，其本質是一張點陣圖圖片，但其中每個“畫素”則表示了實際環境中存在障礙物的概率分佈。

一般來說，採用鐳射雷達、深度攝像頭、超聲波感測器等可以直接測量距離資料的感測器進行SLAM時，可以使用該地圖。這種地圖也可以通過距離測量感測器、超聲波（早期）、鐳射雷達（現在）繪製出來。

（2）.特徵點地圖

特徵點地圖，是用有關的幾何特徵（如點、直線、面）表示環境，常見於vSLAM（視覺SLAM）技術中。

相比柵格地圖，這種地圖看起來就不那麼直觀了。它一般通過如GPS、UWB以及攝像頭配合稀疏方式的vSLAM演算法產生，優點是相對資料儲存量和運算量比較小，多見於最早的SLAM演算法中。

（3）.直接表徵法

直接表徵法中，省去了特徵或柵格表示這一中間環節，直接用感測器讀取的資料來構造機器人的位姿空間。

上圖就是直接記錄了屋子內天花板畫面的影象地圖。這種方法就像衛星地圖一樣，直接將感測器原始資料通過簡單處理拼接形成地圖，相對來說更加直觀。

（4）.拓撲地圖

拓撲地圖，是一種相對更加抽象的地圖形式，它把室內環境表示為帶結點和相關連線線的拓撲結構圖，其中結點表示環境中的重要位置點（拐角、門、電梯、樓梯等），邊表示結點間的連線關係，如走廊等。這種方法只記錄所在環境拓撲連結關係，這類地圖一般是由前幾類地圖通過相關演算法提取得到。

比如掃地機器人要進行房間清掃的時候，就會建立這樣的拓撲地圖：

（5）.小結

在機器人技術中，SLAM的地圖構建通常指的是建立與環境幾何一致的地圖。

一般演算法中建立的拓撲地圖只反映了環境中的各點連線關係，並不能構建幾何一致的地圖，因此，這些拓撲演算法不能被用於SLAM。

直接表徵法類似衛星地圖，它是直接使用感測器（一般是影象感測器）構建得到。這種方法的資訊冗餘度最大，對於資料儲存是很大的挑戰，同時，機器人要從中提取出有用的資料也要耗費一番周折，因此在實際應用中很少使用。

特徵點地圖又是另一個極端，雖然資料量少，但是它往往不能反應所在環境的一些必須的資訊，比如環境中障礙物的位置。vSLAM技術中，多采用這種地圖來解決機器人定位問題。想讓機器人進行自主避障和路徑規劃，還需要額外配置距離感測器，如鐳射雷達、超聲波來完成。

柵格地圖，或者Occupancy Map（佔據地圖）恰好介於其中，一方面它能表示空間環境中的很多特徵，機器人可以用它來進行路徑規劃，另一方面，它又不直接記錄感測器的原始資料，相對實現了空間和時間消耗的最優。因此，柵格地圖是目前機器人所廣泛應用的地圖儲存方式。

2.定位及感測器選擇

當你開啟手機中的導航軟體，在選擇前往目的地的最佳路線之前，首先要做的動作是什麼呢？沒錯，就是定位。我們要先知道自己在地圖中的位置，才可以進行後續的路徑規劃。

在機器人實時定位問題中，由於通過機器人運動估計得到的機器人位置資訊通常具有較大的誤差，我們還需要使用測距單元得到的周圍環境資訊更正機器人的位置。

目前，常見的測距單元包括鐳射測距、超聲波測距以及影象測距三種。其中，憑藉鐳射良好的指向性和高度聚焦性，鐳射雷達已經成為移動機器人的核心感測器，同時它也是目前最可靠、最穩定的定位技術。

自1988年被提出以來，SLAM的理論研究發展十分迅速。在實際應用時，除配備鐳射雷達外，還需要機器人具有IMU（慣性測量單元）、里程計來為鐳射雷達提供輔助資料，這一過程的運算消耗是巨大的，傳統上需要PC級別的處理器，這也成為限制SLAM廣泛應用的瓶頸之一。

3.感測器資料預處理

這是一個完整的SLAM和導航系統的主要架構圖：

其中，SLAM核心過程包括3個步驟，第一步稱為預處理。我們知道，鐳射雷達和其他雷達裝置一樣，某一個時刻只能獲取它所在位置的環境資訊。

這就是我們所說的點雲，它只能反映機器人所在環境中的一個部分。第一步預處理就是對鐳射雷達原始資料進行優化，剔除一些有問題的資料，或者進行濾波。

4.匹配

第二步是匹配，也就是說把當前這一個區域性環境的點雲資料在已經建立地圖上尋找到對應的位置，這個步驟非常關鍵。

這個是ICP的點雲匹配演算法，用於實現匹配。說這個過程關鍵，就是因為它的好壞，直接影響了SLAM構建地圖的精度。這個過程和我們玩拼圖遊戲有點類似，就是在已經拼好的畫面中找到相似之處，確定新的一個拼圖該放在哪裡。

在SLAM過程中，需要將鐳射雷達當前採集的點雲（紅色部分）匹配拼接到原有地圖中。

如果不進行匹配過程，所構建的地圖可能就亂成一團，變成這樣。

5.地圖融合

在這個部分完畢以後，就進行第三步，地圖融合，也就是將這一輪來自鐳射雷達的新資料拼接到原始地圖當中，最終完成地圖的更新。

就像這個圖一樣，這個過程是永遠伴隨SLAM過程的。

資料融合和簡單的貼圖是有很大的差異的。因為實際上感測器描繪的世界存在一定的誤差，或者正巧在這個時間環境有了變化，例如機器人旁邊闖入了一隻小貓。

因此，實際要進行的過程會更加複雜，需要用很多概率演算法，並採用濾波的方式進行融合。將上述這個過程逐次執行，就最終產生了我們看到的柵格地圖。

6.Loop Closure（迴環）問題

這個過程聽起來其實並不複雜，但是要處理好有很大難度。這裡舉幾個例子，比如叫做Loop Closure（迴環）問題。如果匹配演算法不足夠優秀，或者環境中存在很不巧的干擾，當機器人繞著環境一圈後，就會發現原本是應該閉合的一個環形走廊斷開了。

比如正常地圖應該這樣：

如果處理不好，實際地圖就成這樣：

對於環境比較大的場景，迴環問題是不得不面對的，但現實總是不完美的，即使是鐳射雷達這種高精度感測器，也難免存在誤差。而回環問題的難點恰恰在於在一開始出現少許誤差的時候，並不會被發覺，直到機器人繞著環路一圈，隨著誤差的累加，發現環路已經無法閉合時，此時已經釀成大錯，一般很難迴天。

當然這個問題並不是無解，一個好的商用化SLAM系統，迴環問題是否能很好的解決，就成為評判這個系統實力的指標了。

這是前兩天在『思嵐科技』辦公室進行的測試，左邊的視訊是基於開源的ROS機器人作業系統進行的地圖構建，右邊的是基於SLAMWARE構建的地圖。

『視訊暫缺』

當機器人已經繞場一週後，ROS構建的地圖出現了中斷，而SLAMWARE構建的地圖是一個完美的閉環，它與我們辦公室的設計圖完美重合。

除了演算法層面的迴環問題，SLAM實際應用中還有很多這種坑，比如走廊問題與外界干擾問題。

以外界干擾問題來說，通常，鐳射雷達作為機器人的眼睛，一般是安裝在底盤上的，它能看到的視野很有限。當受到外界干擾（人類或者寵物等等）後，機器人很容易丟失定位精度，無法正常完成後續的建圖工作。

當機器人安裝SLAMWARE後，機器人受到干擾，可以完全不受影響，照樣能夠正常工作。

『視訊暫缺』

目前，SLAM的開源實現代表多為學術界，實際應用有很多Corner Case要處理，需要感測器、系統引數、其他輔助裝置的聯合調優。

一般來說，上述的SLAM過程對於運算消耗是巨大的，雖然並沒有達到像訓練神經網路動用伺服器叢集那種地步，但傳統上需要PC級別的處理器。

除配備鐳射雷達外，還需要機器人具有IMU（慣性測量單元）、里程計來為鐳射雷達提供輔助資料，否則SLAM系統也難以得到執行。總的來說，SLAM演算法本身是一個對於外部系統有著多種依賴的演算法，這是一個切實的工程問題。

7.演算法優化及商用

很多機器人，比如掃地機是不可能裝一個PC進去的，為了讓SLAM能在這類裝置裡執行，除了解決鐳射雷達成本外，還要對SLAM演算法做出很好的優化。

瞭解，不是本文主題：

這也是我們思嵐科技SLAMTEC主要的努力方向，一方面，我們這7年多很好的解決了各類實際SLAM演算法難點，另一方面，我們把SLAM這個複雜的系統做了很大的優化，可以放到一個硬幣那麼大的模組內部，降低尺寸功耗。此外，它還集成了IMU等配套感測器，力求做到對於SLAM的使用便捷性。

2.路徑規劃

那麼，機器人利用SLAM技術得到了有效的空間資訊後，它是怎樣實現路徑規劃的？SLAM和路徑規劃之間關係是怎樣的？

實際上，SLAM演算法本身只是完成了機器人的定位和地圖構建兩件事情，與我們說的導航定位並不是完全等價的。這裡的導航，其實是SLAM演算法做不了的。它在業內叫做運動規劃(Motion Planning)。

運動規劃是一個很大的概念，從機械臂的運動、到飛行器的飛行，再到這裡我們說的掃地機的清掃路徑規劃，都是運動規劃的範疇。

我們先談談針對掃地機這類輪式機器人的運動規劃。這裡所需的基礎能力就是路徑規劃，也就是一般在完成SLAM後，要進行一個叫做目標點導航的能力。通俗的說，就是規劃一條從A點到B點的路徑出來，然後讓機器人移動過去。

1.全域性規劃

要實現這個過程，運動規劃要實現至少兩個層次的模組，一個叫做全域性規劃，這個和我們車載導航儀有一點像，它需要在地圖上預先規劃一條線路，也要有當前機器人的位置。這是由我們的SLAM系統提供出來的。行業內一般會用叫做A*的演算法來實現這個過程，它是一種啟發式的搜尋演算法，非常優秀。它最多的應用，是在遊戲中，比如像星際爭霸、魔獸爭霸之類的即時戰略遊戲，都是使用這個演算法來計算單位的運動軌跡的。

2.區域性規劃

當然，僅僅規劃了路徑還是不夠的，現實中會有很多突發情況，比如正巧有個小孩子擋道了，就需要調整原先的路徑。當然，有時候這種調整並不需要重新計算一遍全域性路徑，機器人可能稍微繞一個彎就可以。此時，我們就需要另一個層次的規劃模組，叫做區域性規劃。它可能並不知道機器人最終要去哪，但是對於機器人怎麼繞開眼前的障礙物特別在行。

3.已知地圖（A*）與未知地圖（D*）演算法

這兩個層次的規劃模組協同工作，機器人就可以很好的實現從A點到B點的行動了，不過實際工作環境下，上述配置還不夠。比如A＊演算法規劃的路徑是根據已知地圖，預先規劃好的，一旦機器人前往目的地的過程中遇到了新的障礙物，就只好完全停下來，等待障礙物離開或者重新規劃路徑了。如果掃地機器人買回家，必須先把屋子都走一遍以後才肯掃地，那使用者體驗就會很差。

為此，也會有針對這類演算法的改進，比如SLAMWARE內我們採用改良的D*演算法進行路徑規劃，這也是美國火星探測器採用的核心尋路演算法。這是一種動態啟發式路徑搜尋演算法，它讓機器人在陌生環境中行動自如，在瞬息萬變的環境中游刃有餘。

D*演算法的最大優點是不需要預先探明地圖，機器人可以和人一樣，即使在未知環境中，也可以展開行動，隨著機器人不斷探索，路徑也會時刻調整。

4.空間覆蓋（space coverage）

以上是目前大部分移動機器人都需要的路徑規劃演算法，而掃地機器人作為最早出現在消費市場的服務機器人之一，它需要的路徑規劃演算法更為複雜。

一般來說，掃地機需要這麼幾個規劃能力：貼邊打掃、折返的工字形清掃以及沒電時候自主充電。單單依靠前面介紹的D*這類演算法，無法滿足這些基礎需要。

掃地機器人還需要有額外的規劃演算法，比如針對折返的工字形清掃，有很多問題要處理。掃地機如何最有效進行清掃而不重複清掃？如何讓掃地機和人一樣，理解房間、門、走廊這種概念？

針對這些問題，學術界長久以來有一個專門的研究課題，叫做空間覆蓋（space coverage），同時也提出了非常多的演算法和理論。其中，比較有名的是Morse Decompositions，掃地機通過它實現對空間進行劃分，隨後進行清掃。

20世紀70年代，卡內基梅隴大學（CMU）完全依靠超聲波做到了現在我們掃地機的行為，當然造價也十分昂貴。

前面介紹的從A點到B點移動路徑規劃也是實現這類更高階路徑規劃的基礎。實際上，要從SLAM實現到掃地機器人所需要的這些功能，還是有非常多的工作要做的。

瞭解，不是主題

針對掃地機器人，我們將其特有的路徑規劃功能預先內建在SLAMWARE中，方便廠家進行整合，不需要進行二次開發。