SLAM導(dǎo)航全棧書的正確打開姿勢
SLAM導(dǎo)航全棧書的正確打開姿勢
隨著人工智能、機器人、無人駕駛等技術(shù)的蓬勃發(fā)展，作為底層技術(shù)基石的SLAM也逐漸被大家所熟知。人工智能技術(shù)如果僅僅停留在虛擬的網(wǎng)絡(luò)和數(shù)據(jù)之中的話，那么它挖掘并利用知識的能力將非常有限。也就是說人工智能的最終歸宿是與機器人實體相結(jié)合，實現(xiàn)機器人的完全自主化。而實現(xiàn)自主移動是其中的首要目標(biāo)，目前SLAM導(dǎo)航技術(shù)是實現(xiàn)這一目標(biāo)的熱門研究方向。
與以往的技術(shù)浪潮不同，SLAM是一個軟硬件相結(jié)合、理論加實戰(zhàn)的浩大工程性問題。企業(yè)的目的是要將SLAM技術(shù)真正落地到產(chǎn)品，而不是等你慢慢研究數(shù)學(xué)理論或簡單調(diào)幾個參數(shù)。每個從業(yè)者都應(yīng)該建立起大局觀，以避免重復(fù)造輪子，換句話說我們?nèi)狈?/span>SLAM全棧人才。
圖1 SLAM從業(yè)人員現(xiàn)狀
溫馨提示
1.學(xué)習(xí)SLAM究竟在學(xué)些什么
對于大學(xué)里的學(xué)生，學(xué)習(xí)SLAM大多從看公式推公式開始入手。對于企業(yè)界的工程師們，則多以熱門SLAM框架的代碼解析或移植開始入手。由于SLAM導(dǎo)航技術(shù)在機器人、自動駕駛等方面的廣泛應(yīng)用，所以有大量的從業(yè)者是跨學(xué)科過來的，比如嵌入式、機械設(shè)計、自動化、電子工程、軟件工程等。因此，對于不懂SLAM數(shù)學(xué)理論的從業(yè)者，通常會從學(xué)習(xí)ROS機器人系統(tǒng)編程、傳感器、底盤、驅(qū)動等開始入手。
（1）深入數(shù)學(xué)原理
在高校的學(xué)生，往往跟著教授從SLAM的數(shù)學(xué)公式以及推導(dǎo)學(xué)起。高校教授講課用的書籍通常具備很強的理論性，但大多數(shù)高深的數(shù)學(xué)理論在實際中使用極少，另外實際情況也不見得與數(shù)學(xué)理論模型完全吻合。這打消了許多人的學(xué)習(xí)積極性，感覺學(xué)習(xí)過程費時費力，到頭來又感覺什么也沒學(xué)會。正所謂天下SLAM學(xué)習(xí)者苦數(shù)學(xué)久矣，圖2非常生動形象地反映了他們的心路歷程（嘿嘿）。
圖2 學(xué)習(xí)SLAM數(shù)學(xué)理論的心路歷程
（2）解析熱門框架
在企業(yè)中，工程師往往忙著在Github上收羅各種SLAM開源代碼，找到一些與應(yīng)用需求相符的代碼慢慢逐句研讀。
圖3 代碼中的各種矩陣向量的運算公式都是什么鬼哦
高高興興上Github收羅出一大堆SLAM開源代碼，然后各種選擇恐懼癥都來了。只怪自己專業(yè)水平不夠，只能根據(jù)排名胡亂選擇一個靠前的開源代碼先練手。一頓操作猛如虎，貌似每句代碼都看得懂（不就是一些C++語法嘛，嘿嘿），貌似每句代碼又都看不懂（各種矩陣向量運算，到底在搞什么鬼）。
（3）移植代碼
對于堅持實用主義的學(xué)習(xí)者，管它什么數(shù)學(xué)原理還是代碼邏輯，通通不重要。先搞個別人寫好的項目代碼安裝到自己的電腦跑起來再說，然后順利的話再移植到自己的實際機器人上搞出幾個炫酷的演示效果先唬唬人。
圖4 移植代碼過程中的心酸只有自己知道
在移植代碼前興高采烈，不就是敲幾個命令嘛。在移植代碼時，被各種坑（“對不起您運行的節(jié)點不存在或者找不到路徑，請確保已經(jīng)正確安裝了”、“系統(tǒng)終止了運行，并拋出了一個錯誤”、“由于缺少依賴xxx程序已終止”）折磨的脾氣全無。大部分人是沒有搞懂整個代碼的組織邏輯、加之一些專業(yè)的第三方庫的原理作用不了解、以及SLAM項目的開發(fā)與運行流程不熟。因此從移植代碼中暴露的問題看，也提醒了廣大實用主義者們要惡補一些必要的數(shù)學(xué)理論和代碼解析知識。
（4）學(xué)完ROS很迷茫
對于嵌入式、機械設(shè)計、自動化、電子工程、軟件工程等跨學(xué)科過來的學(xué)習(xí)者，通常最開始就是學(xué)習(xí)ROS機器人系統(tǒng)編程。跟著網(wǎng)上的一些教程，讓小海龜在電腦屏幕上動起來十分令人激動。
圖5 令ROS學(xué)習(xí)者激動的小海龜動起來嘍
恐怕沒有企業(yè)會招聘一個只會使用ROS的工程師吧，畢竟只會使用ROS的話可能什么也做不了。可能很多初學(xué)者聽到ROS機器人操作系統(tǒng)，就被操作系統(tǒng)幾個字嚇住了。其實簡單點說，ROS就是一個分布式的通信機制，幫助程序進程之間更方便地實現(xiàn)通信。解決這種分布式的通信問題正是ROS被設(shè)計出來的初衷，隨著越來越多的人參與ROS開發(fā)及源碼貢獻，涌現(xiàn)了大量的第三方工具和實用開源軟件包，ROS才變成現(xiàn)在的樣子。
一個經(jīng)常讓初學(xué)者困惑的地方是，學(xué)會了ROS就算是學(xué)會機器人開發(fā)了嗎？當(dāng)然不是，嚴(yán)格意義上講ROS只是一套通信機制而已，機器人中的各種算法和應(yīng)用程序依然是用C++/Python等常見編程語言進行開發(fā)。換句話說，學(xué)習(xí)完ROS的代碼組織方式以及通信方式之后，還要再進一步學(xué)習(xí)C++或Python程序開發(fā)，以及掌握必要的底層硬件的驅(qū)動亦或是學(xué)習(xí)上層的SLAM、導(dǎo)航、識別感知等算法。
（5）苦戰(zhàn)各種傳感器融合
對于搞機器人SLAM導(dǎo)航的人來說，萬物皆可融合。有些劍走偏鋒的學(xué)習(xí)者，一上來就各種傳感器、各種卡爾曼融合、各種拼湊，想一想就很辣眼睛。
圖6 各種辣眼睛的傳感器融合方法
當(dāng)然越多的傳感器提供觀測數(shù)據(jù)在理論上是能提升效果的，但毫無原理根據(jù)的胡亂融合往往會起到副作用。在融合各種傳感器的數(shù)據(jù)之前，你最好先搞清楚背后的數(shù)學(xué)原理，不然各種數(shù)據(jù)反而在互相干擾。
2.目前SLAM學(xué)習(xí)的現(xiàn)狀
很多初學(xué)者在學(xué)習(xí)完ROS之后，就不知道下一步該干什么了。而對于搞嵌入式及傳感器的底層開發(fā)者來說，對ROS上層及算法層面軟件的具體工作原理又非常困惑，常常有了解的好奇心但卻無從下手。對于專門研究SLAM算法或?qū)Ш剿惴ǖ难芯咳藛T，他們往往專注于算法層面的某個很細分的領(lǐng)域，一般比較缺乏全局性的工程思維，至于將某項研究成果部署到實際的機器人上落地運行難度就更大了。這就導(dǎo)致各個領(lǐng)域的研究開發(fā)人員都在自己熟悉的領(lǐng)域內(nèi)閉門造車，而缺乏領(lǐng)域之外的必要交流與實踐。軟件層面的開發(fā)者由于缺乏對機器人傳感器、機器人主機和機器人底盤的系統(tǒng)性認識，往往在軟件性能優(yōu)化過程中涉及到軟硬件深度優(yōu)化方面的問題時就束手無策了。而硬件層面的開發(fā)者由于缺乏軟件方面的必備基礎(chǔ)，經(jīng)常在理解軟件層需求時出現(xiàn)偏差。由于缺乏相關(guān)的數(shù)學(xué)理論體系，ROS及硬件相關(guān)領(lǐng)域的開發(fā)人員大多只能充當(dāng)調(diào)參俠，對SLAM導(dǎo)航方面的算法很難有實質(zhì)性的改善。由于缺乏工程思維和實踐經(jīng)驗，SLAM算法或?qū)Ш剿惴ǚ矫娴难芯咳藛T則很難將研究成果落地到實際機器人，甚至SLAM算法研究人員與導(dǎo)航算法研人員也存在不小的交流障礙。
圖7 SLAM學(xué)習(xí)者的現(xiàn)狀
總之在學(xué)習(xí)SLAM的道理上，“重復(fù)造輪子”、“調(diào)參俠”、“調(diào)包黨”、“沉迷于高深理論研究而不能自拔”、“頭疼醫(yī)頭腳疼醫(yī)腳”、“不是在安裝代碼就是在去安裝代碼的路上”、“寫bug改bug然后循環(huán)往復(fù)”、“以為在干算法（真實是瘋狂的調(diào)包黨）”、“一直在入門，從未被精通”這些標(biāo)簽總有一款適合你。這其實也不能怪大家，因為SLAM是一個軟硬件相結(jié)合、理論加實戰(zhàn)的浩大工程性問題。遺憾是，目前國內(nèi)系統(tǒng)講解SLAM的書籍非常之少，SLAM全棧技術(shù)則更是空白，大量涌入的從業(yè)者苦于找不到一本趁手的學(xué)習(xí)寶典。這正是我寫作的初衷，用一本書將SLAM導(dǎo)航中的軟件、硬件、數(shù)學(xué)理論、工程落地等一系列問題一網(wǎng)打盡。提升行業(yè)的整體認知水平，為SLAM導(dǎo)航技術(shù)的普及與落地貢獻微薄力量。
圖8 你可能需要一本SLAM導(dǎo)航全棧書
3.每個從業(yè)者都需要一本SLAM導(dǎo)航的全棧書籍
首先SLAM算法和導(dǎo)航算法是兩大核心技術(shù)，都涉及到理論和實戰(zhàn)兩部分。而這兩大核心技術(shù)的實戰(zhàn)都需要落地到機器人，構(gòu)造一臺真實的機器人既需要硬件基礎(chǔ)知識也需要編程基礎(chǔ)知識。整本書在邏輯上分成四篇：
第一篇講解ROS、C++、OpenCV，讓大家掌握機器人開發(fā)中的必備編程知識。
第二篇講解機器人的傳感器、主機、底盤，讓大家熟悉整個機器人的硬件構(gòu)造以及工作原理。第三篇講解本書的主角SLAM，首先從概率論、狀態(tài)估計、濾波和最優(yōu)理論逐步揭示出SLAM的理論本質(zhì)，然后結(jié)合目前的各種熱門SLAM框架進行代碼實操，讓理論結(jié)合實戰(zhàn)。
第四篇講解自主導(dǎo)航，首先從感知、路徑規(guī)劃、運動控制逐步揭示出自主導(dǎo)航的理論本質(zhì)，然后結(jié)合導(dǎo)航開源代碼以及在真實機器人上的實操讓大家理解SLAM與導(dǎo)航之間的聯(lián)系及應(yīng)用。
圖9 整本書的內(nèi)容組織邏輯
（1）學(xué)習(xí)SLAM數(shù)學(xué)原理
在1986年，Smith和Cheeseman將機器人定位問題和機器人建圖問題放在基于概率論理論框架之下進行統(tǒng)一研究。其中有兩個開創(chuàng)性的點，第一是采用了基于概率論理論框架對機器人的不確定性進行討論，第二是將定位和建圖中的機器人位姿量與地圖路標(biāo)點作為統(tǒng)一的估計量進行整體狀態(tài)估計，這算得上是同時定位與建圖問題研究的起源。
圖10 SLAM定位與建圖中的概率學(xué)
在沒有誤差的理想狀態(tài)，運動軌跡上的機器人位姿可以由運動位移量準(zhǔn)確計算，由于機器人每個位姿都是準(zhǔn)確的并且觀測也是理想情況，觀測路標(biāo)特征的坐標(biāo)也可以準(zhǔn)確計算出來。實際情況是存在誤差的，這就是為什么要用概率模型來表示機器人的運動模型、觀測模型和待求狀態(tài)的原因。
在不確定性條件下，求解SLAM問題中的機器人位姿（即定位）和環(huán)境路標(biāo)（即建圖）其實就是一個狀態(tài)估計過程。由于機器人位姿的偏差，加上觀測誤差的影響，觀測到的路標(biāo)特征坐標(biāo)也自然會偏離于真實路標(biāo)特征坐標(biāo)。也就是說機器人位姿和路標(biāo)特征坐標(biāo)的真實值是無法直接通過觀測信息得到的，因為誤差滲透進了各個地方。在統(tǒng)計意義上，如果能通過不斷調(diào)整機器人位姿和路標(biāo)的取值大小，使機器人位姿和路標(biāo)的當(dāng)前取值基本逼近于真實情況，那么這個不斷調(diào)整機器人位姿和路標(biāo)的取值的迭代過程就是所謂的狀態(tài)估計過程。
圖11 SLAM就是一個狀態(tài)估計問題
用大白話說就是，如果圖11中虛線表示的七角星及三角形與實線表示的七角星及三角形完全重合的話估計就準(zhǔn)確無誤了。其中三角形表示運動過程中機器人位姿，七角星表示運動過程中機器人觀測到的環(huán)境路標(biāo)特征，實線表示真實值，虛線表示估計值。狀態(tài)估計過程之所以有效，是因為機器人位姿與環(huán)境路標(biāo)之間存在某種強有力的關(guān)聯(lián)（也叫約束）。目前大都從兩個思路去構(gòu)建約束，一個思路是在貝葉斯網(wǎng)絡(luò)上構(gòu)建這些約束，另一個思路是在因子圖上構(gòu)建這些約束。
圖12 在貝葉斯網(wǎng)絡(luò)上構(gòu)建SLAM的約束
圖13 在因子圖上構(gòu)建SLAM的約束
不管是貝葉斯網(wǎng)絡(luò)還是因子圖，只是把SLAM的約束構(gòu)建起來了，可以理解是在構(gòu)建方程組。那么接下來就要想辦法求解SLAM約束中的機器人位姿和環(huán)境路標(biāo)，可以理解是在解方程組。由于基于貝葉斯網(wǎng)絡(luò)的SLAM約束一般采用卡爾曼濾波、粒子濾波等方法求解機器人位姿和環(huán)境路標(biāo)，也被俗稱為“濾波方法”。而基于因子圖的SLAM約束一般采用梯度下降以及各種梯度下降變種等優(yōu)化迭代的方法求解機器人位姿和環(huán)境路標(biāo)，也被俗稱為“優(yōu)化方法”。
按照機器人上搭載的傳感器，SLAM算法可以大致分為激光SLAM（對應(yīng)書中的第8章）、視覺SLAM（對應(yīng)書中的第9章）和融合SLAM（對應(yīng)書中的第10章）。而按照SLAM約束求解方法，SLAM算法可以大致分為基于濾波方法的SLAM、基于優(yōu)化方法的SLAM、基于人工智能方法的端到端SLAM等。
（2）通過主流框架進行SLAM實戰(zhàn)
SLAM的發(fā)展日新月異，有大量的開源算法框架不斷涌現(xiàn)，并且同一個算法框架也有大量不同版本的更新迭代。要將市面上出現(xiàn)的所有SLAM算法框架逐一討論一遍既不現(xiàn)實，也沒有必要。其實目前的SLAM具體技術(shù)路線就那么幾個，無外乎激光SLAM、視覺SLAM、多傳感器融合SLAM、與機器學(xué)習(xí)相結(jié)合的SLAM等等。其實目前市面上五花八門的SLAM框架基本上都是在這幾個主要技術(shù)路線的經(jīng)典框架下的微創(chuàng)新，要么對代碼結(jié)構(gòu)做一些重構(gòu)、要么多加幾種傳感器、要么對一些功能模塊進行升級替換，能劃時代做出系統(tǒng)性技術(shù)突破和創(chuàng)新的少之又少。這并不是說咱們這個領(lǐng)域的研究人員不夠努力不上進，恰恰反映了SLAM問題的特性，即需要大量的開發(fā)人員共同參與，進行小步快跑式的遞進式創(chuàng)新，這些微小創(chuàng)新積少成多就能促成一次系統(tǒng)性技術(shù)突破，靠某個單獨的技術(shù)改進就想實現(xiàn)SLAM技術(shù)跨越式的發(fā)展是不太可能的。
圖14 挑選幾個非常有代表性的SLAM實現(xiàn)框架進行討論
因此本書中挑選了幾個非常有代表性的SLAM實現(xiàn)框架進行討論，激光SLAM技術(shù)路線的實現(xiàn)框架包括Gmapping、Cartographer、LOAM，視覺SLAM技術(shù)路線的實現(xiàn)框架包括ORB-SLAM2、LSD-SLAM、SVO，多傳感器融合SLAM技術(shù)路線的實現(xiàn)框架包括RTABMAP、VINS，與機器學(xué)習(xí)相結(jié)合的SLAM技術(shù)路線的實現(xiàn)框架包括CNN-SLAM、DeepVO。到這里肯定有人會質(zhì)疑，區(qū)區(qū)幾百頁書能將這些代碼完全講明白嗎？我的答案是“能”。
分析一個開源代碼，既不是長篇大論逐字逐句地分析每段代碼的作用，也不是像記流水賬式的介紹代碼中每個參數(shù)的配置過程。研究一個開源代碼，應(yīng)該是要從中獲得某些程序設(shè)計思想或者對其進行快速的移植裁剪。因為很多開源代碼本來就存在一大堆問題，其代碼的實現(xiàn)細節(jié)可能并沒有多大學(xué)習(xí)價值，代碼中的眾多參數(shù)大部分可能實際也用不到。想要看代碼細節(jié)含有的完全可以去Github上看代碼注釋或者一些Issue的討論，而有光代碼參數(shù)的配置詳情也在代碼所提供的官網(wǎng)文檔中進行了逐一介紹，所以我在書中采用了三個角度（原理分析、源碼解讀、安裝與運行）來分析各個開源代碼。原理分析，其目的是將前面泛泛而談的SLAM理論落實到具體的算法的具體數(shù)學(xué)公式中來，并給出這些數(shù)學(xué)公式適當(dāng)?shù)膫未a實現(xiàn)，這樣做有利于在接著的源代碼解讀中理解數(shù)學(xué)公式的實現(xiàn)細節(jié)。源碼解讀，其目的是授人以漁，通過對源代碼的輸入輸出數(shù)據(jù)關(guān)系圖、代碼文件組織結(jié)構(gòu)、代碼的總體調(diào)用流程讓大家快速建立起全局性認識，學(xué)會這些方法論后修改代碼或者進行移植裁剪就是小菜一碟了。安裝與運行，其實就是讓大家快速對代碼建立感性認識，對于新手小白來說，將代碼安裝到電腦上并運行起來能極大地建立起學(xué)習(xí)的興趣。
就拿書中關(guān)于Cartographer算法的例子來說，首先從局部建圖、閉環(huán)檢測和全局建圖進行Cartographer原理分析。其中，局部建圖介紹了激光雷達掃描點的幀間匹配、激光雷達掃描點的地圖柵格化過程和地圖數(shù)據(jù)結(jié)構(gòu)。不管是什么SLAM算法，在工程實現(xiàn)上都表現(xiàn)為某個具體的地圖數(shù)據(jù)結(jié)構(gòu)，算法的運算過程基本都圍繞著地圖數(shù)據(jù)結(jié)構(gòu)中的各個數(shù)據(jù)成員以及成員關(guān)系而展開，因此解讀一個SLAM算法，首要任務(wù)就是搞清楚它的地圖數(shù)據(jù)結(jié)構(gòu)是怎樣的。
圖15 Cartographer算法的地圖數(shù)據(jù)結(jié)構(gòu)
然后從cartographer_ros功能包、cartographer核心庫和ceres-solver非線性優(yōu)化庫進行Cartographer源碼解讀。在解讀具體代碼時，除了要搞清楚整個項目文件夾下各個具體文件都是什么用途外，最好還要能從全局上搞懂?dāng)?shù)據(jù)在程序之間是如何流動的，同時跟隨數(shù)據(jù)的輸入輸出關(guān)系搞懂程序中各個類（Class）和函數(shù)（function）的調(diào)用關(guān)系，搞清楚了這些基本就掌握了代碼的一大半了，剩下不過是一些細枝末節(jié)的編程語法細節(jié)罷了。書中通過框圖來生動形象地展示Cartographer程序頂層的數(shù)據(jù)輸入輸出關(guān)系，如圖16。通過流程圖展示Cartographer程序類和函數(shù)的調(diào)用關(guān)系，如圖17。
圖16 Cartographer程序頂層數(shù)據(jù)輸入輸出關(guān)系
圖17 Cartographer程序類和函數(shù)的調(diào)用流程
最后從Cartographer安裝和運行進行具體實戰(zhàn)，在講解Cartographer安裝時分享了我實際遇到的很多坑以及閉坑方法（比如要將src/.rosinstall配置文件中有關(guān)ceres-solver的下載地址更換為Github源，以解決ceres-solver下載慢的問題），對初學(xué)者來說應(yīng)該很有幫助。在講解Cartographer運行時，既有從運行官方的配置啟動文件和測試數(shù)據(jù)集讓大家快速將算法跑起來，又介紹了如何編寫自己的配置啟動文件和通入實際傳感器數(shù)據(jù)來在真實機器人運行算法。
啟動傳感器命令
#啟動激光雷達
roslaunch ydlidar my_x4.launch
#啟動底盤，并發(fā)布輪式里程計
roslaunch xiihoo_bringup minimal.launch
#啟動IMU
roslaunch xiihoo_imu imu.launch
#啟動urdf模型
roslaunch xiihoo_description xiihoo_description.launch
算法配置啟動文件
1 <launch>
2 <node name="cartographer_node" pkg="cartographer_ros"
3 type="cartographer_node" args="
4 -configuration_directory $(find cartographer_ros)/configuration_files
5 -configuration_basename xiihoo_mapbuild.lua"
6 output="screen">
7 <remap from="scan" to="/scan" />
8 <remap from="imu" to="/imu" />
9 <remap from="odom" to="/odom" />
10 </node>
11
12 <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
13 type="cartographer_occupancy_grid_node" args="
14 -resolution 0.05
15 -publish_period_sec 1.0" />
16 </launch>
啟動建圖命令
#啟動建圖
source ~/catkin_ws_carto/install_isolated/setup.bash
roslaunch cartographer_ros xiihoo_mapbuild.launch
#啟動鍵盤遙控
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
#查看地圖
rosrun rviz rviz
圖18 Cartographer在實際機器人上的建圖效果
在具體SLAM算法實戰(zhàn)的過程中，補充講解了很多在第7章SLAM中的數(shù)學(xué)基礎(chǔ)不便于展開的理論知識。比如Gmapping算法實戰(zhàn)中補充講解了RBPF粒子濾波的具體細節(jié)，Cartographer算法實戰(zhàn)中補充講解了閉環(huán)檢測的一種高效方法（分支界定搜索），ORB-SLAM2算法實戰(zhàn)中補充講解了歐拉角、四元數(shù)、旋轉(zhuǎn)矩陣、李群李代數(shù)、多視圖幾何、視覺詞袋模型等重要內(nèi)容，VINS算法實戰(zhàn)中補充講解了多傳感器聯(lián)合標(biāo)定原理、松耦合、緊耦合等，CNN-SLAM和DeepVO算法實戰(zhàn)中補充講解了機器學(xué)習(xí)、神經(jīng)網(wǎng)絡(luò)、深度學(xué)習(xí)等方面的內(nèi)容。
（3）補習(xí)機器人開發(fā)中的編程和硬件基礎(chǔ)知識
在上面的SLAM實戰(zhàn)過程中，涉及到許多編程和硬件相關(guān)知識。比如ROS，幾乎所有的SLAM實戰(zhàn)項目都基于ROS。另外，要用實際的傳感器數(shù)據(jù)來運行SLAM，必須搞清楚IMU、激光雷達、相機、編碼器等傳感器的工作原理以及使用方法，這樣才能標(biāo)定以及正確的驅(qū)動這些傳感器。這時候讀者就需要借助書中的第1~3章來補習(xí)ROS、C++、OpenCV等編程基礎(chǔ)知識，借助書中的第4~6章來補習(xí)機器人傳感器、機器人主機、機器人底盤等硬件基礎(chǔ)知識。
ROS雖然全稱是機器人操作系統(tǒng)，但簡單點理解就是提供了一套分布式的通信機制而已。ROS中可運行程序的基本單位叫節(jié)點，節(jié)點之間通過消息機制進行通信，消息機制有話題（topic）、服務(wù)（service）和動作（action）三種。機器人要實現(xiàn)某個功能（比如SLAM），需要有眾多不同功能的程序之間相互配合完成，ROS的分布式通信機制正是解決這些程序之間相互配合的利器。
圖19 ROS的本質(zhì)就是一套分布式通信機制
話題通信方式是單向異步的，發(fā)布者只負責(zé)將消息發(fā)布到話題，訂閱者只從話題訂閱消息，發(fā)布者與訂閱者之間并不需要事先確定對方的身份，話題充當(dāng)消息存儲容器的角色，這種機制很好地實現(xiàn)了發(fā)布者與訂閱者程序之間的解耦。
圖20 話題通信
服務(wù)通信方式是雙向同步的，服務(wù)客戶端向服務(wù)提供端發(fā)送請求，服務(wù)提供端在收到請求后立馬進行處理并返回響應(yīng)信息。
圖21 服務(wù)通信
動作通信方式是雙向異步的，動作客戶端向動作提供端發(fā)送目標(biāo)后，動作的執(zhí)行需要一個過程（比如處在導(dǎo)航節(jié)點的機器人），動作在被執(zhí)行的過程中會實時反饋狀態(tài)（比如導(dǎo)航過程中機器人的實時位置信息），動作被執(zhí)行完成后會返回結(jié)果（比如已到達導(dǎo)航目標(biāo)點或者導(dǎo)航失敗）。
圖22 動作通信
不難發(fā)現(xiàn)，只要掌握了話題通信，服務(wù)通信和動作通信也很容易上手。我覺得學(xué)習(xí)ROS最大的困難是從0到1的過程，在看了大量學(xué)習(xí)資料后，最重要的是自己親自動手寫一個ROS的程序出來并且編譯調(diào)試全部通過，將程序真正在電腦上跑起來，哪怕只是一個hello_world程序。跟著書中1.5.1節(jié)的例程很容易編寫出一個發(fā)布和訂閱話題消息的ROS程序。
話題發(fā)布程序
1 #include "ros/ros.h"
2 #include "std_msgs/String.h"
3
4 #include <sstream>
5
6 int main(int argc, char **argv)
7 {
8 ros::init(argc, argv, "publish_node");
9 ros::NodeHandle nh;
10
11 ros::Publisher chatter_pub = nh.advertise<std_msgs::String>("chatter", 1000);
12 ros::Rate loop_rate(10);
13 int count = 0;
14
15 while (ros::ok())
16 {
17 std_msgs::String msg;
18
19 std::stringstream ss;
20 ss << "hello " << count;
21 msg.data = ss.str();
22 ROS_INFO("%s", msg.data.c_str());
23
24 chatter_pub.publish(msg);
25
26 ros::spinOnce();
27 loop_rate.sleep();
28 ++count;
29 }
30
31 return 0;
32 }
話題訂閱程序
1 #include "ros/ros.h"
2 #include "std_msgs/String.h"
3
4 void chatterCallback(const std_msgs::String::ConstPtr& msg)
5 {
6 ROS_INFO("I heard: [%s]",msg->data.c_str());
7 }
8
9 int main(int argc, char **argv)
10 {
11 ros::init(argc, argv, "subscribe_node");
12 ros::NodeHandle nh;
13
14 ros::Subscriber chatter_sub = nh.subscribe("chatter", 1000,chatterCallback);
15
16 ros::spin();
17
18 return 0;
19 }
學(xué)會了ROS的話題、服務(wù)和動作通信基本就掌握了ROS的一大半了，剩下在學(xué)習(xí)一些ROS的基本調(diào)試方法和調(diào)試工具就夠了，ROS的調(diào)試工具大致分為命令行工具和可視化工具兩種。ROS命令行工具能在shell終端直接輸入使用，類似于Linux命令，比如roscore、rosrun、roslaunch、rostopic、rosbag、rospack等。ROS可視化工具主要是rviz和rqt，rviz是ROS自帶的三維可視化工具，可以讓用戶通過圖形界面非常方便地開發(fā)調(diào)試ROS。比如可視化顯示激光雷達、深度相機、超聲波等傳感器的數(shù)據(jù)、顯示機器人的三維幾何模型、顯示路徑規(guī)劃實時軌跡、發(fā)送導(dǎo)航目標(biāo)點等。rqt是基于Qt來開發(fā)的，因此rqt用戶可以自由添加和編寫插件來實現(xiàn)自己的功能。
如果說計算機程序是機器人的靈魂，那么硬件本體就是機器人的軀干。熟悉機器人上各個硬件的工作原理，能讓你深入理解機器人中的計算機程序運行以及軟硬件深度優(yōu)化的過程。書中第4~6章將從“機器人傳感器”、“機器人主機”和“機器人底盤”來展開講解，以幫你熟悉機器人開發(fā)過程中必備的硬件基礎(chǔ)知識。
圖23 激光測距
圖24 激光雷達掃描原理
從激光雷達的工作原理的介紹中，很容易理解軟件算法中有關(guān)激光雷達性能參數(shù)的含義（激光線數(shù)、測距頻率、掃描頻率、量程、角度分辨率、測距精度）。同時利用書中介紹的雷達運動畸變校正方法，很容易對激光雷達的運動畸變進行校正。
圖25 激光雷達的運動畸變
同樣通過書中對相機成像原理的介紹，就很容易理解相機內(nèi)參模型以及視覺SLAM中所謂的重投影誤差是個什么東西了。
圖26 單目和雙目相機的成像原理
（4）讓SLAM與導(dǎo)航相結(jié)合
通過上面的一系列學(xué)習(xí)，相信大家都了解了機器人的構(gòu)造并將SLAM算法成功跑起來了，但是光有SLAM如何讓機器人實現(xiàn)自主導(dǎo)航呢？接下你就可以跟著書中的第11~13章將SLAM與自主導(dǎo)航結(jié)合起來，在一臺真實的機器人（xiihoo）上實現(xiàn)自主導(dǎo)航避障了。
圖27 全局路徑規(guī)劃、局部路徑規(guī)劃、軌跡跟蹤
#啟動底盤
roslaunch xiihoo_bringup minimal.launch
#啟動激光雷達
roslaunch ydlidar my_x4.launch
#啟動IMU
roslaunch xiihoo_imu imu.launch
#啟動相機
roslaunch usb_cam usb_cam.launch
#啟動urdf模型
roslaunch xiihoo_description xiihoo_description.launch
#激光建圖
roslaunch cartographer_ros xiihoo_mapbuild.launch
#視覺建圖
rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt mono.yaml
#啟動move_base自主導(dǎo)航
roslaunch xiihoo_nav move_base.launch
圖28 實際建圖效果
參考文獻
[1] 張虎，機器人SLAM導(dǎo)航核心技術(shù)與實戰(zhàn)[M]. 機械工業(yè)出版社，2022.
浙公網(wǎng)安備 33010602011771號