行業(yè)干貨|機器人如何實現(xiàn)自主移動與路徑規(guī)劃？

SLAMTEC思嵐科技

智能服務(wù)機器人正成為行業(yè)的風(fēng)口浪尖，從清掃機器人開始,，家庭陪伴機器人,、送餐機器人等陸續(xù)進入公眾視線。

在討論這類機器人是否能解決實際問題時,，自主定位導(dǎo)航技術(shù)作為機器人智能化的第一步正不斷引起行業(yè)內(nèi)的重視,。同時，作為自主定位導(dǎo)航技術(shù)的重要突破口,，SLAM 技術(shù)也成為關(guān)注焦點,。

正如圖中所示，機器人自主定位導(dǎo)航技術(shù)中包括定位,、地圖創(chuàng)建與路徑規(guī)劃（運動控制）,，而 SLAM 本身只是完成機器人的定位和地圖創(chuàng)建，二者有所區(qū)別,。

7 G& j! C& j( d

那么,，SLAM 技術(shù)究竟是如何實現(xiàn)的？它有哪些難點,？機器人如何實現(xiàn)自主移動和路徑規(guī)劃,？到底什么樣的掃地機器人才算智能？

這些問題,，小編將為你一一解答,，帶領(lǐng)大家了解機器人自主移動的秘密。

5 N) \1 I. l3 @0 {  a1 l5 t" T

今天,，我們先從 SLAM 說起,。

什么是SLAM技術(shù)?

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

* o1 s: g% E$ W( B# i. N

其實,，SLAM 更像是一個概念而不是一個算法,，它本身包含許多步驟，其中的每一個步驟均可以使用不同的算法實現(xiàn),。主要用于解決移動機器人在未知環(huán)境中運行時即時定位與地圖構(gòu)建的問題,。

當(dāng)你身處異地，怎么準確找到想去的地方,？在戶外迷路時,，怎么找到回家的路？沒錯,，我們有導(dǎo)航軟件和戶外地圖,。

9 B) x6 r  F" H, w2 Z# G# ~

和人類繪制地圖一樣，機器人描述環(huán)境,、認識環(huán)境的過程主要就是依靠地圖,。它利用環(huán)境地圖來描述其當(dāng)前環(huán)境信息，并隨著使用的算法與傳感器差異采用不同的地圖描述形式,。

, w5 T+ U  W7 c# U

四種常見的地圖表示方法是什么,？

: F: a' c+ y, `

機器人學(xué)中地圖的表示方法有四種：柵格地圖、特征地圖,、直接表征法以及拓撲地圖,。

柵格地圖

9 T0 j6 Z  q$ q; v+ X, o' J

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

+ q/ F0 `5 _* Z4 T

這種地圖看起來和人們所認知的地圖沒什么區(qū)別,，它最早由 NASA 的 Alberto Elfes 在 1989 年提出，在火星探測車上就用到過,，其本質(zhì)是一張位圖圖片,，但其中每個「像素」則表示了實際環(huán)境中存在障礙物的概率分布。

一般來說,，采用激光雷達,、深度攝像頭、超聲波傳感器等可以直接測量距離數(shù)據(jù)的傳感器進行 SLAM 時,，可以使用該地圖,。這種地圖也可以通過距離測量傳感器、超聲波（早期）,、激光雷達（現(xiàn)在）繪制出來,。

特征點地圖

特征點地圖，是用有關(guān)的幾何特征（如點,、直線,、面）表示環(huán)境,，常見于 vSLAM（視覺 SLAM）技術(shù)中。

7 l$ N. P( O! L. H1 I

相比柵格地圖,，這種地圖看起來就不那么直觀了,。它一般通過如 GPS、UWB 以及攝像頭配合稀疏方式的 vSLAM 算法產(chǎn)生,，優(yōu)點是相對數(shù)據(jù)存儲量和運算量比較小,，多見于最早的 SLAM 算法中。

$ u0 h& f  c' P+ {

直接表征法

直接表征法中，省去了特征或柵格表示這一中間環(huán)節(jié),，直接用傳感器讀取的數(shù)據(jù)來構(gòu)造機器人的位姿空間,。

0 b; X, ~2 b) U# W

. I1 o& N+ c; m  i. U: v* r# K8 ^0 z

上圖就是直接記錄了屋子內(nèi)天花板畫面的圖像地圖。這種方法就像衛(wèi)星地圖一樣,，直接將傳感器原始數(shù)據(jù)通過簡單處理拼接形成地圖，相對來說更加直觀,。

拓撲地圖

拓撲地圖,，是一種相對更加抽象的地圖形式，它把室內(nèi)環(huán)境表示為帶結(jié)點和相關(guān)連接線的拓撲結(jié)構(gòu)圖,，其中結(jié)點表示環(huán)境中的重要位置點（拐角,、門、電梯,、樓梯等）,，邊表示結(jié)點間的連接關(guān)系，如走廊等,。這種方法只記錄所在環(huán)境拓撲鏈接關(guān)系,，這類地圖一般是由前幾類地圖通過相關(guān)算法提取得到。

% g- N. Z  v# l6 d% x: D) d

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

小結(jié)

' I" {: a9 M! d( d" \5 R

在機器人技術(shù)中,，SLAM 的地圖構(gòu)建通常指的是建立與環(huán)境幾何一致的地圖。

一般算法中建立的拓撲地圖只反映了環(huán)境中的各點連接關(guān)系,，并不能構(gòu)建幾何一致的地圖,，因此，這些拓撲算法不能被用于 SLAM,。

$ ?* N* J6 W" a

直接表征法類似衛(wèi)星地圖,，它是直接使用傳感器（一般是圖像傳感器）構(gòu)建得到。這種方法的信息冗余度最大,，對于數(shù)據(jù)存儲是很大的挑戰(zhàn),，同時，機器人要從中提取出有用的數(shù)據(jù)也要耗費一番周折,，因此在實際應(yīng)用中很少使用,。

特征點地圖又是另一個極端,，雖然數(shù)據(jù)量少，但是它往往不能反應(yīng)所在環(huán)境的一些必須的信息,，比如環(huán)境中障礙物的位置,。vSLAM 技術(shù)中，多采用這種地圖來解決機器人定位問題,。想讓機器人進行自主避障和路徑規(guī)劃,，還需要額外配置距離傳感器，如激光雷達,、超聲波來完成,。

1 T2 b. A" k# k$ |

柵格地圖，或者 Occupancy Map（占據(jù)地圖）恰好介于其中,，一方面它能表示空間環(huán)境中的很多特征,，機器人可以用它來進行路徑規(guī)劃，另一方面,，它又不直接記錄傳感器的原始數(shù)據(jù),，相對實現(xiàn)了空間和時間消耗的最優(yōu)。因此,，柵格地圖是目前機器人所廣泛應(yīng)用的地圖存儲方式,。

1 ?! S7 i& ~! f" U- u0 i

思嵐科技的 SLAMWARE 系統(tǒng)內(nèi)部也采用這種地圖方式。

當(dāng)你打開手機中的導(dǎo)航軟件,，在選擇前往目的地的最佳路線之前,，首先要做的動作是什么呢？沒錯,，就是定位,。我們要先知道自己在地圖中的位置，才可以進行后續(xù)的路徑規(guī)劃,。

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

目前,，常見的測距單元包括激光測距,、超聲波測距以及圖像測距三種,。其中，憑借激光良好的指向性和高度聚焦性,，激光雷達已經(jīng)成為移動機器人的核心傳感器,，同時它也是目前最可靠、最穩(wěn)定的定位技術(shù),。

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

6 T2 {2 C, {7 N

那么,，實現(xiàn) SLAM 的過程中,，都會遇到哪些坑呢？

% G3 Q1 i& N6 @: u

SLAM 實現(xiàn)過程中的難點有哪些,？

. M; I6 `3 @; o9 z; p- C8 h0 y

SLAM 主要解決機器人的實時定位與自動建圖問題,，那么，在實際應(yīng)用時,，SLAM 究竟是如何實現(xiàn)的呢,？在實現(xiàn)過程中有哪些難點？

1 B$ H* W2 x+ T: S& V; {

這是一個完整的 SLAM 和導(dǎo)航系統(tǒng)的主要架構(gòu)圖：

  D9 ~$ ?0 E. T; o1 A

# i7 \4 G. e" h9 K) o+ b: Z

其中,，SLAM 核心過程包括 3 個步驟,，第一步稱為預(yù)處理。我們知道,，激光雷達和其他雷達設(shè)備一樣,，某一個時刻只能獲取它所在位置的環(huán)境信息。

這就是我們所說的點云,，它只能反映機器人所在環(huán)境中的一個部分,。第一步預(yù)處理就是對激光雷達原始數(shù)據(jù)進行優(yōu)化，剔除一些有問題的數(shù)據(jù),，或者進行濾波,。

- F6 Z3 e+ a* |$ R9 d* e

6 f* w. u& }: p& Q

第二步是匹配，也就是說把當(dāng)前這一個局部環(huán)境的點云數(shù)據(jù)在已經(jīng)建立地圖上尋找到對應(yīng)的位置,，這個步驟非常關(guān)鍵,。

這個是 ICP 的點云匹配算法，用于實現(xiàn)匹配,。說這個過程關(guān)鍵,，就是因為它的好壞，直接影響了 SLAM 構(gòu)建地圖的精度,。這個過程和我們玩拼圖游戲有點類似,，就是在已經(jīng)拼好的畫面中找到相似之處，確定新的一個拼圖該放在哪里。

在 SLAM 過程中,，需要將激光雷達當(dāng)前采集的點云（紅色部分）匹配拼接到原有地圖中,。

, ^7 X2 x3 y9 k

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

在這個部分完畢以后，就進行第三步,，地圖融合,，也就是將這一輪來自激光雷達的新數(shù)據(jù)拼接到原始地圖當(dāng)中，最終完成地圖的更新,。

4 p$ [- f6 s$ a7 r$ y  }

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

數(shù)據(jù)融合和簡單的貼圖是有很大的差異的,。因為實際上傳感器描繪的世界存在一定的誤差,，或者正巧在這個時間環(huán)境有了變化，例如機器人旁邊闖入了一只小貓,。

7 a/ ]1 v' g- B- S6 X. u! O& u) p+ Y

因此,，實際要進行的過程會更加復(fù)雜，需要用很多概率算法,，并采用濾波的方式進行融合,。將上述這個過程逐次執(zhí)行，就最終產(chǎn)生了我們看到的柵格地圖,。

這個過程聽起來其實并不復(fù)雜,，但是要處理好有很大難度。這里舉幾個例子,，比如叫做 Loop Closure（回環(huán)）問題,。如果匹配算法不足夠優(yōu)秀，或者環(huán)境中存在很不巧的干擾,，當(dāng)機器人繞著環(huán)境一圈后,，就會發(fā)現(xiàn)原本是應(yīng)該閉合的一個環(huán)形走廊斷開了。

比如正常地圖應(yīng)該這樣：

/ a6 q! }, x( n$ }

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

對于環(huán)境比較大的場景,，回環(huán)問題是不得不面對的，但現(xiàn)實總是不完美的,，即使是激光雷達這種高精度傳感器,，也難免存在誤差。而回環(huán)問題的難點恰恰在于在一開始出現(xiàn)少許誤差的時候,，并不會被發(fā)覺,，直到機器人繞著環(huán)路一圈，隨著誤差的累加,，發(fā)現(xiàn)環(huán)路已經(jīng)無法閉合時,，此時已經(jīng)釀成大錯，一般很難回天,。

, Y' }- a6 r6 M: t- ?

當(dāng)然這個問題并不是無解,，一個好的商用化 SLAM 系統(tǒng)，回環(huán)問題是否能很好的解決,，就成為評判這個系統(tǒng)實力的指標了,。

這是前兩天在我們辦公室進行的測試，左邊的視頻是基于開源的 ROS 機器人操作系統(tǒng)進行的地圖構(gòu)建,，右邊的是基于 SLAMWARE 構(gòu)建的地圖,。

# f0 o  a4 X" m  @

# V9 ~+ @! Q, M9 i2 c- t4 _  m7 Y7 ?# `

5 c$ h& T# p, K
# d# O3 ]$ z( `" o! S% J( Q+ m

當(dāng)機器人已經(jīng)繞場一周后，ROS 構(gòu)建的地圖出現(xiàn)了中斷,，而 SLAMWARE 構(gòu)建的地圖是一個完美的閉環(huán),，它與我們辦公室的設(shè)計圖完美重合。

: z6 h7 `6 G5 z9 z8 t% r$ |

除了算法層面的回環(huán)問題,，SLAM 實際應(yīng)用中還有很多這種坑,，比如走廊問題與外界干擾問題。

  H% B! C, Q8 D& @

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

目前，SLAM 的開源實現(xiàn)代表多為學(xué)術(shù)界,，實際應(yīng)用有很多 Corner Case 要處理,，需要傳感器、系統(tǒng)參數(shù),、其他輔助設(shè)備的聯(lián)合調(diào)優(yōu),。

5 I8 \& T3 t: {4 u+ n7 b

一般來說，上述的 SLAM 過程對于運算消耗是巨大的,，雖然并沒有達到像訓(xùn)練神經(jīng)網(wǎng)絡(luò)動用服務(wù)器集群那種地步,，但傳統(tǒng)上需要 PC 級別的處理器。

1 A4 l! N% i- u7 v3 J2 s4 ?

除配備激光雷達外,，還需要機器人具有 IMU（慣性測量單元）,、里程計來為激光雷達提供輔助數(shù)據(jù)，否則 SLAM 系統(tǒng)也難以得到運行�,？偟膩碚f,，SLAM 算法本身是一個對于外部系統(tǒng)有著多種依賴的算法，這是一個切實的工程問題,。

9 o& T; ]! @# v

很多機器人,，比如掃地機是不可能裝一個 PC 進去的，為了讓 SLAM 能在這類設(shè)備里運行,，除了解決激光雷達成本外,，還要對 SLAM 算法做出很好的優(yōu)化。

. z- S- T; I  D( d

那么,，機器人利用 SLAM 技術(shù)得到了有效的空間信息后,，它是怎樣實現(xiàn)路徑規(guī)劃的？SLAM 和路徑規(guī)劃之間關(guān)系是怎樣的,？

( r& N; k4 o! c

機器人如何實現(xiàn)自主移動和路徑規(guī)劃,？

上面我們已經(jīng)知道了 SLAM 與路徑規(guī)劃的關(guān)系。

實際上,，SLAM 算法本身只是完成了機器人的定位和地圖構(gòu)建兩件事情,，與我們說的導(dǎo)航定位并不是完全等價的。這里的導(dǎo)航,，其實是 SLAM 算法做不了的,。它在業(yè)內(nèi)叫做運動規(guī)劃 (Motion Planning)。

* r1 f+ S7 I/ [

運動規(guī)劃是一個很大的概念,，從機械臂的運動,、到飛行器的飛行，再到這里我們說的掃地機的清掃路徑規(guī)劃,，都是運動規(guī)劃的范疇,。

我們先談?wù)勧槍�掃地機這類輪式機器人的運動規(guī)劃。這里所需的基礎(chǔ)能力就是路徑規(guī)劃,，也就是一般在完成 SLAM 后,，要進行一個叫做目標點導(dǎo)航的能力。通俗的說,，就是規(guī)劃一條從 A 點到 B 點的路徑出來,，然后讓機器人移動過去。

7 g# |. k, i% U6 ^

要實現(xiàn)這個過程,，運動規(guī)劃要實現(xiàn)至少兩個層次的模塊,，一個叫做全局規(guī)劃，這個和我們車載導(dǎo)航儀有一點像,，它需要在地圖上預(yù)先規(guī)劃一條線路,，也要有當(dāng)前機器人的位置,。這是由我們的 SLAM 系統(tǒng)提供出來的。行業(yè)內(nèi)一般會用叫做 A * 的算法來實現(xiàn)這個過程,，它是一種啟發(fā)式的搜索算法,，非常優(yōu)秀。它最多的應(yīng)用,，是在游戲中，比如像星際爭霸,、魔獸爭霸之類的即時戰(zhàn)略游戲,，都是使用這個算法來計算單位的運動軌跡的。

當(dāng)然,，僅僅規(guī)劃了路徑還是不夠的,，現(xiàn)實中會有很多突發(fā)情況，比如正巧有個小孩子擋道了,，就需要調(diào)整原先的路徑,。當(dāng)然，有時候這種調(diào)整并不需要重新計算一遍全局路徑,，機器人可能稍微繞一個彎就可以,。此時，我們就需要另一個層次的規(guī)劃模塊,，叫做局部規(guī)劃,。它可能并不知道機器人最終要去哪，但是對于機器人怎么繞開眼前的障礙物特別在行,。

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

為此,，也會有針對這類算法的改進，比如 SLAMWARE 內(nèi)我們采用改良的 D * 算法進行路徑規(guī)劃,，這也是美國火星探測器采用的核心尋路算法,。這是一種動態(tài)啟發(fā)式路徑搜索算法,，它讓機器人在陌生環(huán)境中行動自如，在瞬息萬變的環(huán)境中游刃有余,。

$ L5 x- }0 E: Y% p2 G6 b

D * 算法的最大優(yōu)點是不需要預(yù)先探明地圖,，機器人可以和人一樣，即使在未知環(huán)境中,，也可以展開行動,，隨著機器人不斷探索，路徑也會時刻調(diào)整,。

6 }' L+ O* D. ?: c

以上是目前大部分移動機器人都需要的路徑規(guī)劃算法,，而掃地機器人作為最早出現(xiàn)在消費市場的服務(wù)機器人之一，它需要的路徑規(guī)劃算法更為復(fù)雜,。

  ^1 [2 W3 z& D

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

! I% T2 V+ M0 L9 w

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

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

6 b( I1 K; W9 r. W/ Y

20 世紀 70 年代,，卡內(nèi)基梅隴大學(xué)（CMU）完全依靠超聲波做到了現(xiàn)在我們掃地機的行為，當(dāng)然造價也十分昂貴,。

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

5 y9 P- G: U! x- `
! h: q0 J# y( m) ]3 d/ q

未來第一站

謝謝分享!

韓寒11

家里就有掃地機器人,，很好用的

天天來一回

謝謝分享

simpleplanfx

干貨,，學(xué)習(xí)了

qwchenhao

樓主是否可以談?wù)勍勇輧x在機器人導(dǎo)航中的應(yīng)用。,。

nangonglyj