服务机器人究竟是如何实现自主定位导航的？

      服務(wù)機(jī)器人想要自由行走，實(shí)現(xiàn)自主定位導(dǎo)航是關(guān)鍵，自主定位導(dǎo)航包括定位、建圖與路徑規(guī)劃。SLAM作為機(jī)器人定位導(dǎo)航的核心技術(shù)，正不斷獲得行業(yè)內(nèi)的重視，但SLAM只是完成定位與地圖創(chuàng)建這兩件事，它并不完全等同于自主定位導(dǎo)航。那么，在實(shí)際應(yīng)用時(shí)，SLAM究竟是如何實(shí)現(xiàn)的？在實(shí)現(xiàn)過(guò)程中又有哪些難點(diǎn)？下面將為大家一一解答。

      當(dāng)我們身處異常，導(dǎo)航軟件和戶外地圖成為我們找路最有利的工具，服務(wù)機(jī)器人和人類一樣也需要依靠地圖來(lái)感知外部的環(huán)境，通過(guò)算法與傳感器差異的不同機(jī)器人會(huì)采取不同的地圖描述形式。而柵格地圖是目前機(jī)器人使用最廣泛的地圖存儲(chǔ)方式。

       柵格地圖就是把環(huán)境劃分成一系列柵格，其中每一柵格給定一個(gè)可能值，表示該柵格被占據(jù)的概率。這種地圖看起來(lái)和人們所認(rèn)知的地圖沒(méi)什么區(qū)別，它最早由NASA的Alberto Elfes在1989年提出，在火星探測(cè)車上就用到過(guò)，其本質(zhì)是一張位圖圖片，但其中每個(gè)“像素”則表示了實(shí)際環(huán)境中存在障礙物的概率分布。一般來(lái)說(shuō)，采用激光雷達(dá)、深度攝像頭、超聲波傳感器等可以直接測(cè)量距離數(shù)據(jù)的傳感器進(jìn)行SLAM時(shí)，可以使用該地圖。這種地圖也可以通過(guò)距離測(cè)量傳感器、超聲波（早期）、激光雷達(dá)（現(xiàn)在）繪制出來(lái)。

      SLAM在實(shí)際應(yīng)用時(shí)究竟是如何實(shí)現(xiàn)的？

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

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

?

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

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

?

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

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

?

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

?

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

　　就像這個(gè)圖一樣，這個(gè)過(guò)程是永遠(yuǎn)伴隨SLAM過(guò)程的。

?

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

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

　　這個(gè)過(guò)程聽(tīng)起來(lái)其實(shí)并不復(fù)雜，但是要處理好有很大難度。

　　這里舉幾個(gè)例子，比如叫做loop closure(回環(huán))的問(wèn)題。如果匹配算法不足夠優(yōu)秀，或者環(huán)境中存在很不巧的干擾，當(dāng)機(jī)器人繞著環(huán)境一圈后，就會(huì)發(fā)現(xiàn)原本是應(yīng)該閉合的一個(gè)環(huán)形走廊斷開(kāi)了。

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

?

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

?

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

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

　　這是前兩天在我們辦公室進(jìn)行的測(cè)試，當(dāng)機(jī)器人已經(jīng)繞場(chǎng)一周后，ROS構(gòu)建的地圖出現(xiàn)了中斷，而Slamware模塊構(gòu)建的地圖是一個(gè)完美的閉環(huán)，它與我們辦公室的設(shè)計(jì)圖完美重合。

?     除了算法層面的回環(huán)問(wèn)題，SLAM實(shí)際應(yīng)用中還會(huì)碰到其他坑，比如走廊問(wèn)題與外界干擾問(wèn)題。以外界干擾問(wèn)題來(lái)說(shuō)，通常，激光雷達(dá)作為機(jī)器人的眼睛，一般是安裝在底盤上的，它能看到的視野很有限。當(dāng)受到外界干擾（人類或者寵物等等）后，機(jī)器人很容易丟失定位精度，無(wú)法正常完成后續(xù)的建圖工作。

　　當(dāng)機(jī)器人安裝SLAMWARE后，機(jī)器人受到干擾，可以完全不受影響，照樣能夠正常工作。目前，SLAM的開(kāi)源實(shí)現(xiàn)代表多為學(xué)術(shù)界，實(shí)際應(yīng)用有很多corner case要處理，需要傳感器、系統(tǒng)參數(shù)、其他輔助設(shè)備的聯(lián)合調(diào)優(yōu)。

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

　　除配備激光雷達(dá)外，還需要機(jī)器人具有IMU（慣性測(cè)量單元）、里程計(jì)來(lái)為激光雷達(dá)提供輔助數(shù)據(jù)，否則SLAM系統(tǒng)也難以得到運(yùn)行?？偟膩?lái)說(shuō)，SLAM算法本身是一個(gè)對(duì)于外部系統(tǒng)有著多種依賴的算法，這是一個(gè)切實(shí)的工程問(wèn)題。我們知道很多機(jī)器人，比如掃地機(jī)是不可能裝一個(gè)PC進(jìn)去的。為了讓SLAM能在這類設(shè)備里運(yùn)行，除了解決激光雷達(dá)成本外，還要對(duì)SLAM算法做出很好的優(yōu)化。

這也是我們思嵐科技SLAMTEC主要的努力方向，一方面在SLAM算法上我們這7年多很好的解決了各類實(shí)際算法難點(diǎn)，另一方面，我們把SLAM這個(gè)復(fù)雜的系統(tǒng)做了很大的優(yōu)化，可以放到一個(gè)硬幣那么大的模塊內(nèi)部，降低尺寸功耗。此外，還集成了IMU等配套傳感器，力求做到對(duì)于SLAM的使用的便捷性。

　   如何利用SLAM技術(shù)實(shí)現(xiàn)路徑規(guī)劃的？SLAM和路徑規(guī)劃之間關(guān)系是怎樣的？

      前面我們已經(jīng)提到SLAM算法我們前面已經(jīng)提到過(guò)，它只是是完成了機(jī)器人的定位和地圖構(gòu)建兩件事情，與我們說(shuō)的導(dǎo)航定位并不是完全等價(jià)的。這里的導(dǎo)航，其實(shí)是SLAM算法做不了的。它在業(yè)內(nèi)叫做運(yùn)動(dòng)規(guī)劃(Motion Planning)。

?

　　運(yùn)動(dòng)規(guī)劃是一個(gè)很大的概念，從機(jī)械臂的運(yùn)動(dòng)、到飛行器的飛行，再到這里我們說(shuō)的掃地機(jī)的清掃路徑規(guī)劃，都是運(yùn)動(dòng)規(guī)劃的范疇。

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

?

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

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

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

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

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

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

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

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

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

?

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

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

　　針對(duì)掃地機(jī)器人，SLAMTEC將其特有的路徑規(guī)劃功能預(yù)先內(nèi)置在SLAMWARE中，方便廠家進(jìn)行整合，不需要進(jìn)行二次開(kāi)發(fā)。

      相信通過(guò)以上介紹，大家對(duì)于服務(wù)機(jī)器人的自主定位導(dǎo)航已有一定的了解了，更多干貨敬請(qǐng)期待后期分享！