機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐 課件 第10、11章 移動(dòng)機(jī)器人視覺SLAM、ROS 2.0介紹與編程基礎(chǔ)

第十章移動(dòng)機(jī)器人視覺SLAM機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐目錄content視覺SLAM框架10.110.5ORB-SLAM算法10.2本章小結(jié)稠密建圖10.3其他視覺SLAM算法或框架10.410.1視覺SLAM框架1視覺里程計(jì)視覺里程計(jì)關(guān)心相鄰圖像之間的相機(jī)運(yùn)動(dòng)，即兩圖像之間的運(yùn)動(dòng)關(guān)系。例如，觀察圖，我們會(huì)發(fā)現(xiàn)右圖是左圖向左旋轉(zhuǎn)一定角度的結(jié)果。10.1視覺SLAM框架1視覺里程計(jì)圖像在計(jì)算機(jī)里用一個(gè)數(shù)值矩陣表示。這個(gè)矩陣?yán)锉磉_(dá)著什么東西，計(jì)算機(jī)毫無概念。而在視覺SLAM中，我們只能看到一個(gè)個(gè)像素，知道它們是某些空間點(diǎn)在相機(jī)的成像平面上投影的結(jié)果。所以，為了定量地估計(jì)相機(jī)的運(yùn)動(dòng)，必須先了解相機(jī)與空間點(diǎn)的幾何關(guān)系。10.1視覺SLAM框架1視覺里程計(jì)視覺里程計(jì)需要通過相鄰幀的圖像來估計(jì)相機(jī)的運(yùn)動(dòng)，并恢復(fù)場(chǎng)景的空間結(jié)構(gòu)。視覺里程計(jì)是視覺SLAM框架的關(guān)鍵組成部分，其能把相鄰時(shí)刻的圖像運(yùn)動(dòng)“串”起來，構(gòu)成機(jī)器人的運(yùn)動(dòng)軌跡，從而解決定位問題。另外，其能根據(jù)每個(gè)時(shí)刻的相機(jī)位置，計(jì)算出各像素對(duì)應(yīng)的空間點(diǎn)的位置，從而得到地圖。然而，僅通過視覺里程計(jì)來估計(jì)軌跡，將不可避免地出現(xiàn)累計(jì)漂移（AccumulatingDrift）誤差（簡(jiǎn)稱累計(jì)誤差）問題。這是由于視覺里程計(jì)（在最簡(jiǎn)單的情況下）只估計(jì)兩個(gè)圖像間運(yùn)動(dòng)，而每次估計(jì)都帶有一定的誤差，先前時(shí)刻的誤差將會(huì)傳遞到下一時(shí)刻，導(dǎo)致經(jīng)過一段時(shí)間后，估計(jì)的軌跡不再準(zhǔn)確。10.1視覺SLAM框架2非線性優(yōu)化在移動(dòng)機(jī)器人系統(tǒng)中，各個(gè)傳感器都帶有一定的噪聲。有的傳感器還會(huì)受磁場(chǎng)和溫度的影響。所以，除解決“如何從圖像中估計(jì)出相機(jī)運(yùn)動(dòng)”的問題之外，還要關(guān)心這個(gè)估計(jì)帶有多大的噪聲、這些噪聲是如何從上一時(shí)刻傳遞到下一時(shí)刻的、當(dāng)前的估計(jì)置信度有多大等問題。非線性優(yōu)化要解決的問題是如何從這些帶有噪聲的數(shù)據(jù)中，估計(jì)機(jī)器人系統(tǒng)的狀態(tài)，以及這個(gè)狀態(tài)估計(jì)的不確定性有多大，即要進(jìn)行最大后驗(yàn)概率（Maximum-a-Posteriori，MAP）估計(jì)，也稱為空間狀態(tài)不確定性估計(jì)（SpatialUncertaintyEstimation）。這里的狀態(tài)既包括移動(dòng)機(jī)器人自身的軌跡，也包括地圖。10.1視覺SLAM框架2非線性優(yōu)化在移動(dòng)機(jī)器人視覺SLAM框架中，前端給后端提供待優(yōu)化的數(shù)據(jù)，以及這些數(shù)據(jù)的初始值，后端負(fù)責(zé)系統(tǒng)的優(yōu)化過程，往往面對(duì)的只有數(shù)據(jù)，不必關(guān)心這些數(shù)據(jù)到底來自什么傳感器。前端與計(jì)算機(jī)視覺研究領(lǐng)域更為相關(guān)，需要完成圖像的特征點(diǎn)提取和匹配等，而后端則主要應(yīng)用濾波與非線性優(yōu)化算法。非線性優(yōu)化是SLAM的重要組成部分，其本質(zhì)是對(duì)運(yùn)動(dòng)主體自身和周圍環(huán)境空間不確定性的估計(jì)，利用狀態(tài)估計(jì)理論，把定位和建圖的不確定性表達(dá)出來，然后去估計(jì)狀態(tài)的均值和不確定性（方差）。10.1視覺SLAM框架3回環(huán)檢測(cè)回環(huán)檢測(cè)，又稱閉環(huán)檢測(cè)，主要解決位置估計(jì)隨時(shí)間漂移的問題。移動(dòng)機(jī)器人經(jīng)過一段時(shí)間的運(yùn)動(dòng)后回到了原點(diǎn)，但是由于存在累計(jì)誤差，它的位置估計(jì)值卻沒有回到原點(diǎn)。如果可以讓機(jī)器人知道“回到了原點(diǎn)”，或者把“原點(diǎn)”識(shí)別出來，再把位置估計(jì)值“拉”過去，那么就可以消除累計(jì)誤差了，這就是回環(huán)檢測(cè)。回環(huán)檢測(cè)與“定位”和“建圖”都有密切的關(guān)系。地圖存在的一個(gè)重要意義是為了讓移動(dòng)機(jī)器人知道自己到達(dá)過的地方。為了實(shí)現(xiàn)回環(huán)檢測(cè)，需要讓機(jī)器人具有識(shí)別曾到達(dá)過的地方的能力。最簡(jiǎn)單的方法是，可以在場(chǎng)景中設(shè)置一個(gè)標(biāo)志物（如二維碼圖片），只要移動(dòng)機(jī)器人看到了這個(gè)標(biāo)志物，就表示其回到了原點(diǎn)。但是，應(yīng)用環(huán)境中可能會(huì)限制出現(xiàn)這種輔助標(biāo)志物。10.1視覺SLAM框架4建圖建圖是指構(gòu)建地圖的過程。地圖是對(duì)環(huán)境的描述，根據(jù)不同的SLAM應(yīng)用，地圖的常見形式包括2D柵格地圖、2D拓?fù)涞貓D、3D點(diǎn)云地圖（PointCloudMap）和3D柵格地圖等，如圖所示。10.1視覺SLAM框架4建圖總體上，地圖可以劃分為尺度地圖（MetricMap，也叫度量地圖）、拓?fù)涞貓D（TopologicalMap）和語義地圖（SemanticMap）三種。尺度地圖尺度地圖上的每一點(diǎn)都可以用坐標(biāo)來表示，它強(qiáng)調(diào)精確地表示地圖中物體的位置，通常用稀疏（Sparse）與稠密（Dense）對(duì)它進(jìn)行分類。稀疏地圖進(jìn)行一定程度的抽象，并不需要表達(dá)所有的物體。例如，可以選擇一部分具有代表意義的物體，稱為特征點(diǎn)，也稱為路標(biāo)（Landmark），那么一張稀疏地圖就是由特征點(diǎn)組成的地圖，而不是特征點(diǎn)的部分就可以忽略掉。而稠密地圖著重于建模所有能看到的東西。10.1視覺SLAM框架4建圖總體上，地圖可以劃分為尺度地圖（MetricMap，也叫度量地圖）、拓?fù)涞貓D（TopologicalMap）和語義地圖（SemanticMap）三種。尺度地圖2D尺度地圖是由許多個(gè)小格子（Grid）組成的，3D尺度地圖則是由許多小方塊（Voxel）組成的。一般地，一個(gè)小格子或小方塊有占據(jù)、空閑、未知三種狀態(tài)，以表達(dá)該小格子或小方塊內(nèi)是否有物體。當(dāng)查詢某個(gè)空間位置時(shí)，地圖能夠給出該位置是否可以通過的信息。但是，這種地圖需要存儲(chǔ)每個(gè)小格子或小方塊的狀態(tài)，耗費(fèi)大量的存儲(chǔ)空間，而且多數(shù)情況下地圖的許多細(xì)節(jié)部分是無用的。10.1視覺SLAM框架4建圖總體上，地圖可以劃分為尺度地圖（MetricMap，也叫度量地圖）、拓?fù)涞貓D（TopologicalMap）和語義地圖（SemanticMap）三種。拓?fù)涞貓D相比于尺度地圖的精確性，拓?fù)涞貓D則更強(qiáng)調(diào)地圖元素之間的關(guān)系。拓?fù)涞貓D是一種圖論中的圖（Graph），由頂點(diǎn)和邊組成，僅考慮頂點(diǎn)間的連通性，用邊來連接相鄰的頂點(diǎn)，其只考慮兩點(diǎn)之間是否是連通的，而不考慮如何從一點(diǎn)到達(dá)另一點(diǎn)。比如，從地鐵路線圖中，我們可以知道華中科技大學(xué)站與光谷大道站相連，這個(gè)地鐵路線圖便是一種拓?fù)涞貓D。10.1視覺SLAM框架4建圖總體上，地圖可以劃分為尺度地圖（MetricMap，也叫度量地圖）、拓?fù)涞貓D（TopologicalMap）和語義地圖（SemanticMap）三種。語義地圖語義地圖上的每個(gè)地點(diǎn)和道路都會(huì)用標(biāo)簽的集合來表示，它是一種對(duì)環(huán)境（室內(nèi)或室外）的增強(qiáng)表示，同時(shí)包含了幾何信息和高層次的定性特征。語義地圖一般是建立在尺度地圖之上的，其能將空間中物體的語義屬性與其周圍環(huán)境的幾何感知建立聯(lián)系，進(jìn)而供人們利用。10.2ORB-SLAM算法ORB-SLAM是西班牙Zaragoza大學(xué)的RaulMur-Artal編寫的基于稀疏特征點(diǎn)的視覺SLAM算法，其論文ORB-SLAM:aversatileandaccuratemonocularSLAMsystem發(fā)表在2015年的IEEETransactiononRobotics上。目前開源代碼的版本包括ORB-SLAM、ORB-SLAM2和ORB-SLAM3。最早的ORB-SLAM版本主要用于單目視覺SLAM，從第二個(gè)版本開始支持單目相機(jī)、雙目相機(jī)和RGB-D深度相機(jī)三種接口的視覺傳感器，支持使用ROS。其代碼結(jié)構(gòu)清晰，命名規(guī)范，非常適合移植到實(shí)際項(xiàng)目中。10.2ORB-SLAM算法ORB-SLAM基于圖像特征識(shí)別，可以實(shí)時(shí)運(yùn)行，適用于室內(nèi)或室外，小場(chǎng)景或大場(chǎng)景，包括跟蹤、局部建圖、回環(huán)檢測(cè)等模塊。ORB-SLAM使用ORB（OrientedFASTandRotatedBRIEF）算子進(jìn)行特征點(diǎn)提取與描述，具有旋轉(zhuǎn)不變性，在計(jì)算時(shí)間上比SIFT算法快兩個(gè)數(shù)量級(jí)，比SURF算法快一個(gè)數(shù)量級(jí)，同時(shí)與它們保持相似的檢測(cè)效果。而且ORB-SLAM對(duì)噪聲有較強(qiáng)的抗干擾能力，因此，ORB-SLAM具有很強(qiáng)的跟蹤魯棒性和可持續(xù)性，可以很好地處理劇烈運(yùn)動(dòng)的圖像，也可以很好地處理回環(huán)檢測(cè)、重定位和位置初始化。10.2ORB-SLAM算法ORB-SLAM算法框架如圖所示，包括三個(gè)線程：跟蹤、局部建圖和回環(huán)檢測(cè)。ORB-SLAM算法中的視覺里程計(jì)是基于特征點(diǎn)檢測(cè)算法實(shí)現(xiàn)的，利用ORB-SLAM算法的實(shí)時(shí)性，可以實(shí)時(shí)計(jì)算出相機(jī)的旋轉(zhuǎn)和平移數(shù)據(jù)，并根據(jù)特征點(diǎn)恢復(fù)出3D稀疏地圖，進(jìn)行建圖和定位。10.2ORB-SLAM算法跟蹤線程跟蹤線程作為ORB-SLAM算法的前端，負(fù)責(zé)特征點(diǎn)跟蹤，不斷獲取相鄰圖像幀之間的位姿變化，同時(shí)通過投影變換，生成關(guān)鍵幀（KeyFrame）可視區(qū)間內(nèi)的地圖點(diǎn)（MapPoint），更新地圖關(guān)系，并確定關(guān)鍵幀插入的時(shí)機(jī)。該線程首先進(jìn)行建圖的初始化過程，同時(shí)計(jì)算兩個(gè)模型：?jiǎn)螒?yīng)矩陣（HomographyMatrix）模型和基礎(chǔ)矩陣（FundamentalMatrix）模型，并對(duì)兩個(gè)模型進(jìn)行評(píng)分，根據(jù)各自分值評(píng)估，使用兩者之一：若視差較小，則采用單應(yīng)矩陣模型恢復(fù)運(yùn)動(dòng)；反之，則采用基礎(chǔ)矩陣模型恢復(fù)運(yùn)動(dòng)。恢復(fù)運(yùn)動(dòng)后，采用集束調(diào)整（BundleAdjustment，即BA優(yōu)化，也叫光束平差法）優(yōu)化初始化結(jié)果。10.2ORB-SLAM算法跟蹤線程初始化完成后，跟蹤線程從圖像中提取ORB特征點(diǎn)，并默認(rèn)從前一幀，采用勻速運(yùn)動(dòng)模型或關(guān)鍵幀模型計(jì)算相機(jī)的位姿，最后進(jìn)行姿態(tài)估計(jì)。若初始化后第一幀跟蹤失敗，則需通過全局重定位來初始化位姿估計(jì)，此時(shí)當(dāng)前幀會(huì)與所有關(guān)鍵幀進(jìn)行匹配，通過不斷優(yōu)化得到重定位后的位姿。最后，根據(jù)關(guān)鍵幀插入的條件，判斷是否需要插入新的關(guān)鍵幀。10.2ORB-SLAM算法局部建圖線程局部建圖線程主要完成局部地圖構(gòu)建。首先根據(jù)新加入的關(guān)鍵幀更新共視圖（CovisibilityGraph）、生長(zhǎng)樹（SpanningTree，也叫擴(kuò)展樹），同時(shí)對(duì)跟蹤線程中生成的地圖點(diǎn)進(jìn)行剔除，從而控制地圖中的地圖點(diǎn)數(shù)量。隨后，在共視圖中，依據(jù)三角原理生成新的地圖點(diǎn)，對(duì)當(dāng)前幀、共視圖中與之相關(guān)聯(lián)的關(guān)鍵幀能夠檢測(cè)到的地圖點(diǎn)進(jìn)行局部BA優(yōu)化。最后，再剔除冗余的關(guān)鍵幀。10.2ORB-SLAM算法局部建圖線程其中，共視圖是無向加權(quán)圖，如圖所示。共視圖中每個(gè)節(jié)點(diǎn)是一個(gè)關(guān)鍵幀，如果兩個(gè)關(guān)鍵幀之間滿足一定的共視關(guān)系（如至少有15個(gè)共視地圖點(diǎn)），就將它們連成一條邊，邊的權(quán)重是共視地圖點(diǎn)的數(shù)目。共視圖是ORB-SLAM中的一個(gè)核心概念，在視覺SLAM算法的搜索地圖點(diǎn)、回環(huán)檢測(cè)、重定位和優(yōu)化中起著重要的作用。10.2ORB-SLAM算法局部建圖線程生長(zhǎng)樹只針對(duì)關(guān)鍵幀而言，在關(guān)鍵幀建立時(shí)，需確定生長(zhǎng)樹的父子關(guān)系，如圖所示。生長(zhǎng)樹除用于更新局部地圖關(guān)鍵幀外，還用于回環(huán)校正。10.2ORB-SLAM算法回環(huán)檢測(cè)線程回環(huán)檢測(cè)線程是消除累計(jì)誤差最有效的辦法?；丨h(huán)檢測(cè)線程主要分為兩個(gè)過程：回環(huán)條件檢測(cè)和回環(huán)校正?；丨h(huán)條件檢測(cè)的主要工作是將當(dāng)前幀與候選回環(huán)幀中所有的幀進(jìn)行比較，根據(jù)設(shè)置的條件進(jìn)行比對(duì)及評(píng)分，當(dāng)?shù)玫匠鲩撝档脑u(píng)分后，即得到回環(huán)幀，并開始進(jìn)行非線性優(yōu)化。10.2ORB-SLAM算法回環(huán)檢測(cè)線程回環(huán)校正的主要工作是在當(dāng)前幀和回環(huán)幀之間找到更多的對(duì)應(yīng)點(diǎn)，通過這些對(duì)應(yīng)點(diǎn)計(jì)算Sim3變換，求解當(dāng)前幀和回環(huán)幀之間的平移和旋轉(zhuǎn)數(shù)據(jù)，如圖所示。Sim3變換使用3對(duì)不共線的匹配點(diǎn)進(jìn)行相似變換（SimilarityTransformation），求解出兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)矩陣、平移向量和尺度變換。回環(huán)校正時(shí)，局部建圖線程和全局BA優(yōu)化需停止，以完成本質(zhì)圖（EssentialGraph）優(yōu)化和回環(huán)融合。10.2ORB-SLAM算法回環(huán)檢測(cè)線程其中，本質(zhì)圖也叫本征圖，也只針對(duì)關(guān)鍵幀而言，本質(zhì)圖比共視圖更稀疏，其作用是在回環(huán)校正時(shí)，用相似變換來矯正尺度漂移，把回環(huán)誤差均攤在本質(zhì)圖中，如圖所示。本質(zhì)圖中的連接邊更少，僅保留聯(lián)系緊密的邊，使得結(jié)果更精確。同時(shí)，相比全局BA優(yōu)化，本質(zhì)圖也可以快速收斂且結(jié)果更精確。10.2ORB-SLAM算法回環(huán)檢測(cè)線程本質(zhì)圖中包含：生長(zhǎng)樹的連接關(guān)系；形成回環(huán)的連接關(guān)系，即回環(huán)檢測(cè)后，地圖點(diǎn)發(fā)生變動(dòng)后的新增連接關(guān)系；共視關(guān)系非常好（至少有100個(gè)共視地圖點(diǎn)）的連接關(guān)系。10.2ORB-SLAM算法ORB-SLAM算法采用g2o（GeneralGraphOptimization，通用圖優(yōu)化）庫(kù)作為非線性優(yōu)化工具，支持全局優(yōu)化和局部?jī)?yōu)化，能有效地減少對(duì)特征點(diǎn)位置和自身位姿的估計(jì)誤差。另外，其采用詞袋算法，使用DBoW2庫(kù)（DBoW2是一種高效的回環(huán)檢測(cè)算法，稱為BagsofBinaryWordsforFastPlaceRecognitioninImageSequence）進(jìn)行回環(huán)檢測(cè)，減少了尋找特征點(diǎn)的計(jì)算量，同時(shí)回環(huán)匹配和重定位效果好。例如，當(dāng)機(jī)器人遇到一些意外情況時(shí)，數(shù)據(jù)流突然被打斷了，ORB-SLAM算法可以在短時(shí)間內(nèi)重新把機(jī)器人在地圖中定位出來。10.2ORB-SLAM算法ORB-SLAM算法也存在一些缺點(diǎn)，主要如下。構(gòu)建出的地圖是稀疏點(diǎn)云圖，保留了圖像中特征點(diǎn)的一部分作為關(guān)鍵點(diǎn)，固定在空間中進(jìn)行定位，很難描繪地圖中的障礙物的存在。初始化時(shí)機(jī)器人需要保持低速運(yùn)動(dòng)，相機(jī)需要對(duì)準(zhǔn)特征和幾何紋理豐富的物體。相機(jī)旋轉(zhuǎn)時(shí)比較容易丟幀，對(duì)噪聲敏感，不具備尺度不變性。如果使用純視覺SLAM用于機(jī)器人導(dǎo)航，容易產(chǎn)生累計(jì)誤差，精度不高，因此需要結(jié)合IMU，提高精度。10.2ORB-SLAM算法相比ORB-SLAM，ORB-SLAM2增加了一個(gè)線程，將整個(gè)系統(tǒng)分成四個(gè)相對(duì)獨(dú)立的線程，各線程之間通過關(guān)鍵幀連接。這四個(gè)線程具體為：跟蹤線程、局部建圖線程、回環(huán)檢測(cè)線程、全局BA優(yōu)化線程。其中，前三個(gè)線程并行執(zhí)行，保證SLAM的實(shí)時(shí)性，第四個(gè)線程依據(jù)回環(huán)檢測(cè)的結(jié)果進(jìn)行全局BA調(diào)整（當(dāng)檢測(cè)到回環(huán)時(shí)，回環(huán)融合后會(huì)觸發(fā)全局BA優(yōu)化線程），優(yōu)化全局地圖和軌跡。ORB-SLAM2算法框架如圖所示。10.3稠密建圖1空間地圖的表示方式目前空間地圖的表示方式主要有稀疏特征點(diǎn)地圖、稠密地圖、占據(jù)柵格地圖，這三種地圖均有不同的應(yīng)用場(chǎng)景，各有優(yōu)勢(shì)。在空間環(huán)境建模時(shí)，應(yīng)根據(jù)具體的應(yīng)用場(chǎng)景選用不同的地圖，以達(dá)到最高效率。稀疏特征點(diǎn)地圖稀疏特征點(diǎn)地圖主要分為稀疏路標(biāo)地圖及稀疏點(diǎn)云地圖兩種，如圖所示。10.3稠密建圖1空間地圖的表示方式目前空間地圖的表示方式主要有稀疏特征點(diǎn)地圖、稠密地圖、占據(jù)柵格地圖，這三種地圖均有不同的應(yīng)用場(chǎng)景，各有優(yōu)勢(shì)。在空間環(huán)境建模時(shí)，應(yīng)根據(jù)具體的應(yīng)用場(chǎng)景選用不同的地圖，以達(dá)到最高效率。稠密地圖稠密地圖目前主要分為稠密點(diǎn)云地圖、TSDF（TruncatedSignedDistanceFunction，截?cái)喾?hào)距離函數(shù)）地圖、面元（SurfaceElement，Surfel）地圖，如圖所示。10.3稠密建圖1空間地圖的表示方式目前空間地圖的表示方式主要有稀疏特征點(diǎn)地圖、稠密地圖、占據(jù)柵格地圖，這三種地圖均有不同的應(yīng)用場(chǎng)景，各有優(yōu)勢(shì)。在空間環(huán)境建模時(shí)，應(yīng)根據(jù)具體的應(yīng)用場(chǎng)景選用不同的地圖，以達(dá)到最高效率。占據(jù)柵格地圖占據(jù)柵格地圖分為3D占據(jù)柵格地圖和2D占據(jù)柵格地圖，如圖所示。其中，3D占據(jù)柵格地圖也稱為3D八叉樹（Octo-tree）地圖。10.3稠密建圖1空間地圖的表示方式從前面介紹的空間地圖的表示方式可以看出，在機(jī)器人SLAM系統(tǒng)中，地圖的作用包括定位、導(dǎo)航、避障、重建、交互等，需要根據(jù)具體的場(chǎng)景選用不同的地圖，以滿足功能需求。隨著計(jì)算機(jī)視覺和圖像處理領(lǐng)域的飛速發(fā)展，視覺SLAM還可以實(shí)現(xiàn)語義地圖構(gòu)建、人機(jī)交互等復(fù)雜多元的功能，基于視覺圖像語義分割構(gòu)建的語義3D地圖如圖所示。10.3稠密建圖1空間地圖的表示方式激光雷達(dá)的優(yōu)勢(shì)是能夠準(zhǔn)確測(cè)量場(chǎng)景中物體之間的距離，可測(cè)量的距離遠(yuǎn)、抗干擾能力強(qiáng)，常用于室內(nèi)外大場(chǎng)景的三維重建。3D激光雷達(dá)可以獲得多個(gè)平面上的信息，拓展了2D激光雷達(dá)的應(yīng)用場(chǎng)景。3D激光雷達(dá)也是無人駕駛汽車中的主要傳感器之一。圖是使用3D激光雷達(dá)構(gòu)建的無人駕駛高精度地圖。10.3稠密建圖1空間地圖的表示方式構(gòu)建地圖需要完成兩方面的工作：3D點(diǎn)云拼接和語義特征點(diǎn)提取。通常使用基于圖優(yōu)化的SLAM算法進(jìn)行分層細(xì)化，然后對(duì)3D點(diǎn)云進(jìn)行拼接，使用機(jī)器學(xué)習(xí)和人工監(jiān)督的迭代算法提取語義特征。下圖是基于3D激光雷達(dá)的場(chǎng)景感知效果。無人駕駛系統(tǒng)獲得行人，車輛的體積、速度，與車輛的距離等信息后，進(jìn)而實(shí)現(xiàn)定位、導(dǎo)航、避障等復(fù)雜功能。10.3稠密建圖1空間地圖的表示方式受限于常規(guī)3D激光雷達(dá)的垂直視場(chǎng)角，移動(dòng)平臺(tái)周邊存在一定的錐形盲區(qū)。目前所使用的激光雷達(dá)多是動(dòng)態(tài)掃描式的，機(jī)械旋轉(zhuǎn)結(jié)構(gòu)的穩(wěn)定性難以保證，信息獲取頻率較低。車規(guī)級(jí)的激光雷達(dá)常采用固態(tài)激光雷達(dá)和數(shù)字激光雷達(dá)，硬件穩(wěn)定性有所提升。為了解決單一傳感器的局限性，人們使用多種類別的傳感器進(jìn)行多層次的數(shù)據(jù)融合，從而提高建圖的準(zhǔn)確性和魯棒性。這些傳感器有相機(jī)（單目相機(jī)、多目相機(jī)、深度相機(jī)）、2D/3D激光雷達(dá)、IMU、里程計(jì)、北斗衛(wèi)星導(dǎo)航系統(tǒng)、GPS（GlobalPositioningSystem，全球定位系統(tǒng)）等。多傳感器融合的SLAM的缺點(diǎn)是數(shù)據(jù)融合復(fù)雜度較大，計(jì)算量較大，維護(hù)困難。10.3稠密建圖1空間地圖的表示方式激光雷達(dá)和相機(jī)在建圖方面的優(yōu)缺點(diǎn)如表所示。10.3稠密建圖1空間地圖的表示方式下表從地圖重構(gòu)效率、環(huán)境內(nèi)容完整性、碰撞檢測(cè)效率及典型算法方面對(duì)比了多種類型的地圖。其中，柵格地圖及特征地圖是傳統(tǒng)激光SLAM算法中常用的地圖類型，常應(yīng)用于室內(nèi)導(dǎo)航系統(tǒng)中，能夠較準(zhǔn)確地描述環(huán)境的幾何信息，但缺乏語義信息，限制了機(jī)器人的智能化水平。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定一般來說，雙目立體視覺的標(biāo)定精度主要采用對(duì)標(biāo)準(zhǔn)件測(cè)量得到的測(cè)量精度、3D空間中某一特征點(diǎn)三維重建的精度，以及通過標(biāo)定實(shí)驗(yàn)測(cè)量得到的相機(jī)出廠參數(shù)間接來評(píng)價(jià)。本節(jié)通過左右圖像平面上實(shí)測(cè)角點(diǎn)與其在相對(duì)圖像平面上對(duì)應(yīng)極線的匹配程度來評(píng)價(jià)雙目立體視覺的標(biāo)定精度，該方法充分考慮兩個(gè)圖像特征之間的匹配對(duì)應(yīng)關(guān)系。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定2D-2D映射：對(duì)極幾何對(duì)極幾何是同一個(gè)視頻序列中兩個(gè)圖像之間內(nèi)在的射影幾何，它只依賴于相機(jī)相對(duì)位置及其內(nèi)參，獨(dú)立于圖像中的場(chǎng)景結(jié)構(gòu)。雙目相機(jī)的標(biāo)定利用了對(duì)極幾何模型。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定2D-2D映射：對(duì)極幾何對(duì)極幾何由3×3的矩陣F表示，稱為基礎(chǔ)矩陣（FundamentalMatrix）。對(duì)極幾何的本質(zhì)是兩個(gè)圖像與以相機(jī)基線為軸的平面束相交的幾何關(guān)系。如圖所示，左右相機(jī)的光心O1、O2分別為左右相機(jī)的投影中心，I1和I2為圖像平面，三維空間點(diǎn)P及其在兩個(gè)圖像平面視圖上的成像點(diǎn)P1與P2都處在同一個(gè)平面上，這個(gè)平面稱為對(duì)極平面。連接左右相機(jī)投影中心的線為基線O1O2，基線O1O2與左右圖像平面相交的點(diǎn)稱為極點(diǎn)，分別為e1、e2。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定2D-2D映射：對(duì)極幾何投影點(diǎn)p1和對(duì)應(yīng)極點(diǎn)e1的連線為極線，同理，p2和e2的連線也為極線。左右極線分別用l1、l2表示。圖像點(diǎn)p1經(jīng)過反投影，對(duì)應(yīng)三維空間中的一條射線，這條射線由p1和相機(jī)矩陣T確定。將射線投影至第二個(gè)圖像上會(huì)形成一條直線，可見三維空間點(diǎn)P在第二個(gè)圖像上的成像點(diǎn)p2必然在極線l2上。因此，第一個(gè)圖像上的成像點(diǎn)p1與第二個(gè)圖像上的極線l2之間存在著映射：這個(gè)映射的本質(zhì)是基礎(chǔ)矩陣F表示的從p1到l2的射影變換，基礎(chǔ)矩陣F也是對(duì)極幾何的核心。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定2D-2D映射：對(duì)極幾何設(shè)點(diǎn)P在左相機(jī)坐標(biāo)系下的空間位置為設(shè)矢量

，

，

，

，右相機(jī)坐標(biāo)系相對(duì)于左相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣為R，平移向量為，則有：由于P1與p1同向，P2與p2同向，因而有：10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定2D-2D映射：對(duì)極幾何式中s1、s2為常量，可得：上式兩側(cè)同時(shí)對(duì)t做外積，可得：再同時(shí)左乘

可得：10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定2D-2D映射：對(duì)極幾何由于和均處于對(duì)極平面上，因此，代入上式可得：式中，

，稱為本質(zhì)矩陣（EssentialMatrix），反映了點(diǎn)P在左右相機(jī)坐標(biāo)系下的成像點(diǎn)之間的對(duì)應(yīng)關(guān)系。若已知點(diǎn)P在左相機(jī)坐標(biāo)系下成像點(diǎn)p1的坐標(biāo)，則可根據(jù)上式求得其在右相機(jī)坐標(biāo)系下成像點(diǎn)p2的坐標(biāo)，反之亦然。定義：p1、p2在像素坐標(biāo)系下的矢量分別為

、

，根據(jù)針孔成像原理，有：10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定2D-2D映射：對(duì)極幾何K為相機(jī)內(nèi)參矩陣，根據(jù)

，可得：式中，

，稱為基礎(chǔ)矩陣，內(nèi)含雙目相機(jī)之間位姿信息，反映了點(diǎn)P在左右像素坐標(biāo)系下的成像點(diǎn)之間的對(duì)應(yīng)關(guān)系。若已知點(diǎn)P在左像素坐標(biāo)系下成像點(diǎn)的坐標(biāo)，則可根據(jù)上式求得其在右像素坐標(biāo)系下成像點(diǎn)

的坐標(biāo)，反之亦然。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定2D-2D映射：對(duì)極幾何對(duì)極約束也常用于計(jì)算單目相機(jī)拍攝的兩個(gè)圖像之間的相對(duì)位姿，并三角化一些三維特征點(diǎn)，算法包括如下步驟：分別檢測(cè)兩個(gè)圖像中的特征點(diǎn)，常用ORB算法；使用五點(diǎn)法或八點(diǎn)法，根據(jù)匹配的特征點(diǎn)對(duì)像素坐標(biāo)，求出E或F；通過奇異值分解，根據(jù)E或F，求出雙目相機(jī)之間的相對(duì)位姿

；通過三角化計(jì)算特征點(diǎn)三維坐標(biāo)。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定雙目相機(jī)外參標(biāo)定雙目相機(jī)的外參主要指的是左右相機(jī)之間的相對(duì)位姿，雙目相機(jī)外參標(biāo)定即求解左右兩個(gè)相機(jī)坐標(biāo)系之間的變換關(guān)系。若在單目相機(jī)標(biāo)定過程中，已求得單應(yīng)性矩陣

及相機(jī)內(nèi)參矩陣K，則可求得比例因子

：10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定雙目相機(jī)外參標(biāo)定設(shè)相機(jī)坐標(biāo)系到世界坐標(biāo)系的旋轉(zhuǎn)矩陣

，根據(jù)旋轉(zhuǎn)矩陣的性質(zhì)有

，可求得旋轉(zhuǎn)矩陣R及平移向量t，即相機(jī)此時(shí)相對(duì)于標(biāo)定板的外參。在雙目相機(jī)外參標(biāo)定過程中，若分別求得左右相機(jī)相對(duì)于同一標(biāo)定板的外參R1、t1、Rr和tr，則世界坐標(biāo)系下某一點(diǎn)P在左右相機(jī)坐標(biāo)系下的坐標(biāo)向量p1和pr可以表示為消去P，可得：10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定雙目相機(jī)外參標(biāo)定設(shè)有左相機(jī)坐標(biāo)系到右相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣

和平移向量

：根據(jù)上式，若同時(shí)求解出左右相機(jī)相對(duì)于同一標(biāo)定板的外參，即可求得兩相機(jī)之間的外參。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定3D-2D映射：PnPPnP（Perspective-n-Point，n點(diǎn)透視）是求解3D到2D點(diǎn)對(duì)運(yùn)動(dòng)的方法。當(dāng)已知一組（n對(duì)）三維特征點(diǎn)及它們?cè)谝晥D中的像素坐標(biāo)（3D-2D點(diǎn)對(duì)）時(shí)，利用該方法可求解相機(jī)姿態(tài)，這樣的對(duì)應(yīng)關(guān)系可以由特征點(diǎn)匹配算法得到。在使用雙目相機(jī)或RGB-D相機(jī)情況下，可以直接使用PnP求解姿態(tài)。若視覺傳感器是單目相機(jī)，首先要利用對(duì)極約束初始化出一些三維特征點(diǎn)，才能使用PnP。PnP可以在很少的匹配點(diǎn)對(duì)中獲得較好的運(yùn)動(dòng)估計(jì)，能減輕計(jì)算壓力。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定3D-2D映射：PnP典型的PnP問題有多種求解方法，經(jīng)常使用的是三對(duì)點(diǎn)估計(jì)位姿的P3P方法。如圖所示，輸入數(shù)據(jù)為三對(duì)3D-2D匹配點(diǎn)，3D點(diǎn)為世界坐標(biāo)系中的A、B、C，2D點(diǎn)為相機(jī)坐標(biāo)系中的a、b、c，相機(jī)光心為O。10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定3D-2D映射：PnP圖中的三角形存在如下對(duì)應(yīng)關(guān)系：由余弦定理可得：將上述三式兩邊同除以

，并記

，

，可得：10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定3D-2D映射：PnP記

，

，

，可得：進(jìn)一步可得：10.3稠密建圖2雙目相機(jī)幾何模型與標(biāo)定3D-2D映射：PnP從P3P原理中可以看出，為了求解PnP，需利用三角形相似的性質(zhì)，求解投影點(diǎn)a、b、c在相機(jī)坐標(biāo)系下的3D坐標(biāo)，從而把問題轉(zhuǎn)換成一個(gè)3D到3D的位姿估計(jì)問題。然而，P3P也存在以下問題。P3P只利用三個(gè)點(diǎn)的信息，當(dāng)給定的配對(duì)點(diǎn)多于三對(duì)時(shí)，其難以利用更多的信息。如果3D點(diǎn)或2D點(diǎn)受噪聲影響，或者存在誤匹配，則算法失效。10.3稠密建圖3稠密建圖基于單目相機(jī)的稠密三維重建難點(diǎn)在于快速實(shí)時(shí)恢復(fù)稠密的深度圖像，完成這一過程需要進(jìn)行大量的計(jì)算，算法需要在效率和精度之間保持平衡。通常選取兩幀具有一定視差的圖像，通過立體視覺算法快速計(jì)算每個(gè)像素的深度值。一個(gè)標(biāo)準(zhǔn)的方法是為第一幀圖像的每一個(gè)像素x1在第二幀圖像對(duì)應(yīng)的極線上搜索最相似的像素x2，這個(gè)過程稱為極線搜索。在搜索過程中，通過比較像素周圍局部圖像塊A,B的相似度來確定最佳匹配。10.3稠密建圖3稠密建圖常用圖像塊差異性或相似度的度量方法如下。SAD（SumofAbsoluteDifference），即用圖像塊每個(gè)像素灰度差的絕對(duì)值和度量：SSD（SumofSquaredDistance），即用圖像塊每個(gè)像素灰度差的平方和度量：NCC（NormalizedCrossCorrelation），即用圖像塊之間的相關(guān)性度量：10.3稠密建圖3稠密建圖綜上所述，單目稠密建圖的深度估計(jì)完整過程如下：假設(shè)圖像上的像素深度整體上滿足均勻－高斯混合分布。通過極線搜索和塊匹配確定重投影像素的位置。根據(jù)搜索結(jié)果三角化出每個(gè)像素的深度值并計(jì)算不確定性。將恢復(fù)的深度圖像與上一次估計(jì)相融合。若收斂，則結(jié)束算法，否則返回步驟（2）。10.3稠密建圖3稠密建圖KinectFusion稠密建圖算法的流程如下。深度圖像轉(zhuǎn)換：將獲得的深度圖像轉(zhuǎn)換成點(diǎn)云，根據(jù)相鄰像素點(diǎn)求得每個(gè)點(diǎn)的法向量。相機(jī)跟蹤：計(jì)算全局模型在上一幀相機(jī)位姿下的深度圖像，將該深度圖像和當(dāng)前幀深度圖像都轉(zhuǎn)換成點(diǎn)云，通過ICP（IterativeClosestPoint，迭代最近點(diǎn)）算法，配準(zhǔn)求得兩幀間的相對(duì)運(yùn)動(dòng)。體素化表示：采用截?cái)喾?hào)距離函數(shù)（TruncatedSignedDistanceFunction，TSDF）表示三維模型，不斷融合深度數(shù)據(jù)，將待重建的三維空間均勻劃分成網(wǎng)格，每個(gè)體素中存儲(chǔ)其中心到最近的三維物體表面的距離。經(jīng)步驟（2）后，將當(dāng)前幀點(diǎn)云融合到三維模型中。10.3稠密建圖3稠密建圖KinectFusion稠密建圖算法的流程如下。光線投射：計(jì)算在當(dāng)前視角下場(chǎng)景的可見表面，用來和下一幀圖像進(jìn)行配準(zhǔn)，計(jì)算下一幀相機(jī)的位姿。注意：在步驟（3）中，KinectFusion稠密建圖算法采用GPU更新網(wǎng)格中的值，只要顯卡滿足重建的幀率不低于深度相機(jī)的幀率，就能做到實(shí)時(shí)稠密三維重建。10.3稠密建圖3稠密建圖但KinectFusion稠密建圖算法存在以下局限性。使用ICP算法進(jìn)行相機(jī)位姿跟蹤，十分依賴場(chǎng)景的紋理豐富程度。基于Kinect相機(jī)，預(yù)設(shè)TSDF中的三維網(wǎng)格，限制了場(chǎng)景尺度。沒有提供類似ORB-SLAM算法的回環(huán)檢測(cè)?；谶@些問題，人們后來對(duì)KinectFusion稠密建圖算法進(jìn)行了改進(jìn)。例如，為減少對(duì)紋理的依賴性，融合ICP和RGB-D深度相機(jī)跟蹤方法；為突破場(chǎng)景尺度限制，令三維網(wǎng)格隨相機(jī)運(yùn)動(dòng)，讓三維網(wǎng)格外的區(qū)域不參與計(jì)算；采用DBoW算法進(jìn)行回環(huán)檢測(cè)，提高三維稠密建圖的準(zhǔn)確性。10.4其他視覺SLAM算法或框架1LSD-SLAM基于圖像特征識(shí)別的視覺SLAM屬于特征點(diǎn)法，特征點(diǎn)法又稱為兩視圖幾何（Two-ViewGeometry）法，該算法通過提取圖像的特征點(diǎn)，對(duì)前后幀圖像的特征點(diǎn)進(jìn)行匹配，進(jìn)而恢復(fù)相機(jī)的位姿。特征點(diǎn)法較為成熟，能夠獲得較精確的位姿估計(jì)。在處理輸入圖像時(shí)，首先提取圖像中的特征點(diǎn)，然后在求解過程中，最小化特征點(diǎn)的幾何位姿誤差。特征點(diǎn)法會(huì)丟失圖像中許多有效信息，特別是在弱紋理場(chǎng)景中，特征點(diǎn)法無法提取有效的特征點(diǎn)，將失效。10.4其他視覺SLAM算法或框架1LSD-SLAMLSD-SLAM（LargeScaleDirectmonocularSLAM）與DSO（DirectSparseOdometry）屬于直接法視覺SLAM，能夠?qū)崟r(shí)構(gòu)建稠密或半稠密的點(diǎn)云地圖。總體來說，直接法的計(jì)算開銷要高于特征點(diǎn)法，但精度略差于特征點(diǎn)法。對(duì)于稀疏的特征點(diǎn)地圖來說，人或其他系統(tǒng)難以直接利用和理解地圖信息，而使用直接法視覺SLAM構(gòu)建的稠密地圖，可以直觀地呈現(xiàn)出三維模型，為機(jī)器人的導(dǎo)航與避障提供了有利條件。10.4其他視覺SLAM算法或框架1LSD-SLAMLSD-SLAM算法的主要優(yōu)點(diǎn)如下。LSD-SLAM是針對(duì)像素進(jìn)行的，體現(xiàn)了像素梯度與直接法的關(guān)系，以及像素梯度與極線方向在稠密地圖重建中的角度關(guān)系。LSD-SLAM在CPU上實(shí)現(xiàn)了半稠密地圖的實(shí)時(shí)重建。LSD-SLAM具有較好的實(shí)時(shí)性與穩(wěn)定性。10.4其他視覺SLAM算法或框架1LSD-SLAMLSD-SLAM運(yùn)行效果如圖所示。圖中上半部分為估計(jì)的軌跡與地圖；下半部分為圖像中被建模的部分，具有較明顯的像素梯度分布，即半稠密特點(diǎn)。10.4其他視覺SLAM算法或框架2SVOSVO（Semi-directVisualOdometry）是由Forster等人于2014年提出的一種基于稀疏半直接法的視覺里程計(jì)算法。半直接法是將特征點(diǎn)法與直接法結(jié)合起來的方法，SVO跟蹤一些梯度明顯的關(guān)鍵點(diǎn)（角點(diǎn)，但沒有描述子），然后像直接法那樣，根據(jù)這些關(guān)鍵點(diǎn)周圍的信息，估計(jì)相機(jī)運(yùn)動(dòng)及它們的位置，如圖所示。10.4其他視覺SLAM算法或框架2SVO開源版SVO存在一些問題，需要針對(duì)不同的應(yīng)用場(chǎng)景進(jìn)行優(yōu)化。由于SVO的目標(biāo)應(yīng)用平臺(tái)為無人機(jī)的俯視相機(jī)，視野內(nèi)的物體主要是地面，而且相機(jī)的運(yùn)動(dòng)主要為水平運(yùn)動(dòng)和垂直運(yùn)動(dòng)，因此SVO的許多細(xì)節(jié)是圍繞這個(gè)應(yīng)用背景進(jìn)行設(shè)計(jì)的，使得它在平視相機(jī)中表現(xiàn)不佳。另外，SVO在關(guān)鍵幀選擇時(shí)，僅使用平移量作為確定新關(guān)鍵幀的策略，而沒有考慮旋轉(zhuǎn)量。這在無人機(jī)俯視情況下是有效的，但在平視相機(jī)中則會(huì)容易丟失關(guān)鍵幀。所以，如果要在平視相機(jī)中使用SVO，必須對(duì)其加以修改。10.4其他視覺SLAM算法或框架2SVO開源版SVO存在一些問題，需要針對(duì)不同的應(yīng)用場(chǎng)景進(jìn)行優(yōu)化。SVO為了速度快和輕量化，舍棄了非線性優(yōu)化和回環(huán)檢測(cè)部分，也基本沒有建圖功能。這意味著SVO的位姿估計(jì)結(jié)果中必然存在累計(jì)誤差，而且在丟失圖像后不太容易進(jìn)行重定位（因?yàn)闆]有描述子用來回環(huán)檢測(cè)）。10.4其他視覺SLAM算法或框架3OpenVSLAMORB-SLAM、LSD-SLAM和DSO等視覺SLAM算法在可用性和擴(kuò)展性方面還需要進(jìn)一步優(yōu)化，在應(yīng)用于機(jī)器人或無人機(jī)的地圖構(gòu)建和3D建模，以及移動(dòng)設(shè)備上的增強(qiáng)現(xiàn)實(shí)（AugmentedReality，AR）等方面時(shí)不方便。因此，提供一個(gè)開源、易用、擴(kuò)展性好的視覺SLAM框架就顯得非常有意義。OpenVSLAM是日本國(guó)立先進(jìn)工業(yè)科學(xué)技術(shù)研究所（NationalInstituteofAdvancedIndustrialScienceandTechnology）在2019年提出的一種基于特征點(diǎn)法的視覺SLAM框架，并在GitHub上進(jìn)行了開源。10.4其他視覺SLAM算法或框架3OpenVSLAM如圖所示，OpenVSLAM是一種可以基于單目相機(jī)、雙目相機(jī)、RGB-D深度相機(jī)的視覺SLAM系統(tǒng)，其主要特點(diǎn)如下。兼容多種相機(jī)類型，可以處理各種類型的相機(jī)模型，擴(kuò)展方便。可以存儲(chǔ)和加載預(yù)先構(gòu)建的地圖，進(jìn)行定位。API易于理解，組件封裝了多個(gè)函數(shù)，易于使用，模塊化好。10.4其他視覺SLAM算法或框架3OpenVSLAMOpenVSLAM框架是基于具有稀疏特征的間接SLAM算法，如圖所示，其包括三個(gè)模塊：跟蹤模塊、建圖模塊和全局優(yōu)化模塊。其中，跟蹤模塊主要實(shí)現(xiàn)ORB特征提取和位姿估計(jì)，以及關(guān)鍵點(diǎn)檢測(cè)；建圖模塊主要實(shí)現(xiàn)三維重建，以及局部位姿和三維點(diǎn)信息的優(yōu)化；全局優(yōu)化模塊主要實(shí)現(xiàn)回環(huán)檢測(cè)及全局優(yōu)化。10.4其他視覺SLAM算法或框架4VINS-Fusion由于受到視覺傳感器的限制，單目視覺SLAM算法存在很大的局限性。具有尺度不確定性：無法確定地圖中的尺度與實(shí)際尺度之間的比例關(guān)系。在弱光或強(qiáng)光場(chǎng)景下，相機(jī)曝光不足或過度曝光，無法獲取有效的圖像信息。在低紋理或少特征場(chǎng)景下，畫面包含的信息量太少，無法準(zhǔn)確估計(jì)相機(jī)運(yùn)動(dòng)。相機(jī)的快速運(yùn)動(dòng)造成圖像模糊甚至畫面撕裂，無法進(jìn)行可靠的跟蹤。10.4其他視覺SLAM算法或框架4VINS-Fusion香港科技大學(xué)的QinT和LiP等人提出了基于緊耦合非線性優(yōu)化的單目視覺慣性里程計(jì)算法VINS-Mono，該算法將IMU預(yù)積分測(cè)量與視覺測(cè)量以緊耦合的形式組合，得到最大后驗(yàn)概率估計(jì)問題，采用非線性優(yōu)化的方法估計(jì)最優(yōu)狀態(tài)。該算法具備以下優(yōu)點(diǎn)。是一種適應(yīng)性強(qiáng)且具有魯棒性的初始化算法。利用光流法跟蹤特征點(diǎn)，減輕了相機(jī)快速運(yùn)動(dòng)帶來的不良影響。在狀態(tài)估計(jì)中考慮了相機(jī)的卷簾快門效應(yīng)，并進(jìn)行了有效補(bǔ)償。在狀態(tài)估計(jì)中，能夠在線校準(zhǔn)相機(jī)-IMU的外參及二者之間的時(shí)間偏移，顯著提高在未知的傳感器組件上進(jìn)行SLAM的精度。通過詞袋模型進(jìn)行特征點(diǎn)匹配，能夠進(jìn)行回環(huán)檢測(cè)與全局位姿優(yōu)化。10.4其他視覺SLAM算法或框架4VINS-Fusion在VINS-Mono的基礎(chǔ)上，QinT和LiP等人又開發(fā)了VINS-Fusion算法，該算法支持多種傳感器組合，并且能夠使用GPS提供的絕對(duì)位姿進(jìn)行測(cè)量，進(jìn)一步提高全局路徑的精度。因此，VINS-Fusion算法是一種基于視覺、IMU及部分全局傳感器（GPS、磁力計(jì)、氣壓計(jì)）緊耦合的視覺SLAM算法。VINS-Fusion是VINS-Mono的擴(kuò)展，其根據(jù)傳感器的配置及使用場(chǎng)景，分別實(shí)現(xiàn)了單目相機(jī)+IMU、僅雙目相機(jī)、雙目相機(jī)+IMU，以及雙目相機(jī)+IMU+GPS四種視覺SLAM。VINS-Fusion算法在室內(nèi)外不同場(chǎng)景下均可運(yùn)行，在一些極端情況（燈光很暗）下也能正常運(yùn)行，具有較好的魯棒性和精度。10.4其他視覺SLAM算法或框架4VINS-FusionVINS-Fusion算法框圖如圖所示。10.4其他視覺SLAM算法或框架4VINS-Fusion下圖是地球坐標(biāo)系，是以移動(dòng)機(jī)器人或無人駕駛汽車當(dāng)前所在位置點(diǎn)為坐標(biāo)原點(diǎn)的坐標(biāo)系。坐標(biāo)系X軸指向東邊，Y軸指向北邊，Z軸指向天空。10.4其他視覺SLAM算法或框架4VINS-Fusion地球坐標(biāo)系采用三維直角坐標(biāo)系來描述地球表面，實(shí)際應(yīng)用較為困難，一般使用簡(jiǎn)化后的二維投影坐標(biāo)系來描述。其中，統(tǒng)一橫軸墨卡托（TheUniversalTransverseMercator，UTM）坐標(biāo)系是一種應(yīng)用較為廣泛的二維投影坐標(biāo)系，如圖所示。10.5本章小結(jié)本章首先介紹了移動(dòng)機(jī)器人視覺SLAM的整體框架，以及經(jīng)典的ORB-SLAM算法。然后，分析了幾種不同空間地圖表示方式，以及相機(jī)稠密建圖的具體步驟。最后介紹了一些常見視覺SLAM算法或框架，擴(kuò)展讀者的視野，幫助讀者了解相關(guān)技術(shù)發(fā)展趨勢(shì)。第十一章ROS2.0介紹與編程基礎(chǔ)機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐目錄contentROS2.0設(shè)計(jì)思想11.1ROS2.0安裝與使用11.2ROS2.0編程基礎(chǔ)11.3本章小結(jié)11.411.1ROS2.0設(shè)計(jì)思想1ROS1.0問題總結(jié)ROS最初是基于PR2機(jī)器人設(shè)計(jì)的，這款機(jī)器人搭載了當(dāng)時(shí)最先進(jìn)的移動(dòng)計(jì)算平臺(tái)，而且網(wǎng)絡(luò)性能優(yōu)異，不需要考慮實(shí)時(shí)性問題，主要應(yīng)用于科研領(lǐng)域。如今應(yīng)用ROS的機(jī)器人領(lǐng)域越來越廣：輪式機(jī)器人、人形機(jī)器人、機(jī)械臂、室外機(jī)器人（如無人駕駛汽車）、無人飛行器、救援機(jī)器人等，美國(guó)NASA甚至使用ROS開發(fā)火星探測(cè)器，機(jī)器人開始從科研領(lǐng)域走向人們的日常生活。11.1ROS2.0設(shè)計(jì)思想1ROS1.0問題總結(jié)ROS雖然仍是機(jī)器人領(lǐng)域的開發(fā)利器，但由于最初設(shè)計(jì)時(shí)具有局限性，也逐漸暴露出不少問題。沒有構(gòu)建多機(jī)器人系統(tǒng)：多機(jī)器人系統(tǒng)是機(jī)器人領(lǐng)域研究的一個(gè)重點(diǎn)問題，可以解決單機(jī)器人性能不足、無法應(yīng)用等問題，但是ROS1.0中并沒有構(gòu)建多機(jī)器人系統(tǒng)的標(biāo)準(zhǔn)方法。不支持跨平臺(tái)：ROS1.0基于Linux系統(tǒng)，在Windows、MacOS、RTOS等系統(tǒng)上無法應(yīng)用或功能有限，這對(duì)機(jī)器人開發(fā)者和開發(fā)工具提出了較高要求。11.1ROS2.0設(shè)計(jì)思想1ROS1.0問題總結(jié)ROS雖然仍是機(jī)器人領(lǐng)域的開發(fā)利器，但由于最初設(shè)計(jì)時(shí)具有局限性，也逐漸暴露出不少問題。不具有實(shí)時(shí)性：很多應(yīng)用場(chǎng)景下的機(jī)器人對(duì)實(shí)時(shí)性有較高要求，尤其在工業(yè)領(lǐng)域，系統(tǒng)需要做到硬實(shí)時(shí)的性能指標(biāo)，但是ROS1.0缺少實(shí)時(shí)性方面的設(shè)計(jì)，所以在很多應(yīng)用中捉襟見肘。11.1ROS2.0設(shè)計(jì)思想1ROS1.0問題總結(jié)ROS雖然仍是機(jī)器人領(lǐng)域的開發(fā)利器，但由于最初設(shè)計(jì)時(shí)具有局限性，也逐漸暴露出不少問題。限制網(wǎng)絡(luò)連接：ROS1.0的分布式機(jī)制需要良好的網(wǎng)絡(luò)環(huán)境來保證數(shù)據(jù)完整性，網(wǎng)絡(luò)數(shù)據(jù)的丟失或延遲會(huì)導(dǎo)致ROS通信問題；而且網(wǎng)絡(luò)沒有數(shù)據(jù)加密、安全防護(hù)等功能，網(wǎng)絡(luò)中的任意主機(jī)都可獲得節(jié)點(diǎn)發(fā)布或接收的消息數(shù)據(jù)。生產(chǎn)環(huán)境不穩(wěn)定：ROS1.0的穩(wěn)定性欠佳，ROSMaster、節(jié)點(diǎn)等重要環(huán)節(jié)在很多情況下會(huì)不穩(wěn)定，導(dǎo)致很多機(jī)器人應(yīng)用系統(tǒng)從研究開發(fā)到消費(fèi)產(chǎn)品的過渡非常艱難。11.1ROS2.0設(shè)計(jì)思想1ROS1.0問題總結(jié)ROS雖然仍是機(jī)器人領(lǐng)域的開發(fā)利器，但由于最初設(shè)計(jì)時(shí)具有局限性，也逐漸暴露出不少問題。缺乏項(xiàng)目管理：ROS1.0沒有為一些新特性（如部署的生命周期管理和靜態(tài)配置）提供清晰的模式和支持工具。11.1ROS2.0設(shè)計(jì)思想1ROS1.0問題總結(jié)除此之外，構(gòu)建ROS2.0的原因還有改進(jìn)原有ROS面向用戶的API。目前存在的大量ROS代碼都與2009年2月發(fā)布的0.4版MangoTango的客戶端庫(kù)兼容。從穩(wěn)定性的角度來看，這很好，但也意味著ROS應(yīng)用會(huì)被其限制，因?yàn)楫?dāng)年的API設(shè)計(jì)概念已經(jīng)無法適用于當(dāng)今的需求。因此，在ROS2.0中，新的API將被設(shè)計(jì)，但關(guān)鍵概念（分布式處理、匿名發(fā)布/訂閱消息傳遞、帶有反饋的RPC、語言中立性、系統(tǒng)自省性等）將保持不變。11.1ROS2.0設(shè)計(jì)思想2ROS2.0發(fā)展現(xiàn)狀ROS2.0的發(fā)展歷程如圖所示。截至2021年底，ROS2.0已經(jīng)更迭了10個(gè)版本，最新發(fā)布的兩個(gè)版本是2021年5月發(fā)布的GalacticGeochelone和2020年6月發(fā)布的FoxyFitzroy，前者為過渡版本，維護(hù)時(shí)間為1年，后者為穩(wěn)定版本，維護(hù)時(shí)間為3年。11.1ROS2.0設(shè)計(jì)思想2ROS2.0發(fā)展現(xiàn)狀ROS2.0繼承了ROS1.0提高機(jī)器人研發(fā)中的軟件復(fù)用率的設(shè)計(jì)目標(biāo)，并在此之上提出了另外五個(gè)目標(biāo)。支持多機(jī)器人系統(tǒng)：ROS2.0增加了對(duì)多機(jī)器人系統(tǒng)的支持，提高了多機(jī)器人之間通信的網(wǎng)絡(luò)性能，更多機(jī)器人系統(tǒng)及應(yīng)用將出現(xiàn)在ROS社區(qū)中?？缭搅嗽团c產(chǎn)品之間的鴻溝：ROS2.0不僅針對(duì)科研領(lǐng)域，還關(guān)注機(jī)器人從研究到應(yīng)用之間的過渡，可以讓更多機(jī)器人直接搭載ROS2.0系統(tǒng)走向市場(chǎng)。11.1ROS2.0設(shè)計(jì)思想2ROS2.0發(fā)展現(xiàn)狀ROS2.0繼承了ROS1.0提高機(jī)器人研發(fā)中的軟件復(fù)用率的設(shè)計(jì)目標(biāo)，并在此之上提出了另外五個(gè)目標(biāo)。支持微控制器：ROS2.0不僅可以運(yùn)行在現(xiàn)有的X86和ARM系統(tǒng)上，還將支持MCU等嵌入式微控制器，如常用的ARM-M4、ARM-M7內(nèi)核。支持實(shí)時(shí)控制：ROS2.0加入了對(duì)實(shí)時(shí)控制的支持，可提高控制的時(shí)效性和機(jī)器人的整體性能。11.1ROS2.0設(shè)計(jì)思想2ROS2.0發(fā)展現(xiàn)狀ROS2.0繼承了ROS1.0提高機(jī)器人研發(fā)中的軟件復(fù)用率的設(shè)計(jì)目標(biāo)，并在此之上提出了另外五個(gè)目標(biāo)?？缦到y(tǒng)平臺(tái)支持：ROS2.0不僅能運(yùn)行在Linux系統(tǒng)之上，還支持Windows和MacOS等系統(tǒng)，讓開發(fā)者有更多的選擇。11.1ROS2.0設(shè)計(jì)思想3ROS2.0通信模型ROS1.0的通信模型主要包含主題（Topic）、服務(wù)（Service）等通信機(jī)制，ROS2.0的通信模型會(huì)稍顯復(fù)雜，加入了很多DDS（DataDistributionService）的通信機(jī)制，如圖所示。11.1ROS2.0設(shè)計(jì)思想3ROS2.0通信模型基于DDS的ROS2.0通信模型包含以下幾個(gè)關(guān)鍵概念。參與者（Participant）在DDS中，每一個(gè)發(fā)布者或者訂閱者都稱為參與者，對(duì)應(yīng)于一個(gè)使用DDS的用戶，可以使用某種定義好的數(shù)據(jù)類型來讀寫全局?jǐn)?shù)據(jù)空間。發(fā)布者（Publisher）數(shù)據(jù)發(fā)布的執(zhí)行者，支持多種數(shù)據(jù)類型的發(fā)布，可以與多個(gè)數(shù)據(jù)寫入器相連，發(fā)布一種或多種主題的消息。11.1ROS2.0設(shè)計(jì)思想3ROS2.0通信模型基于DDS的ROS2.0通信模型包含以下幾個(gè)關(guān)鍵概念。訂閱者（Subscriber）數(shù)據(jù)訂閱的執(zhí)行者，支持多種數(shù)據(jù)類型的訂閱，可以與多個(gè)數(shù)據(jù)讀取器（DataReader）相連，訂閱一種或多種主題的消息。數(shù)據(jù)寫入器（DataWriter）上層應(yīng)用向發(fā)布者更新數(shù)據(jù)的對(duì)象，每個(gè)數(shù)據(jù)寫入器對(duì)應(yīng)一個(gè)特定的主題，類似于ROS1.0中的一個(gè)消息發(fā)布者。11.1ROS2.0設(shè)計(jì)思想3ROS2.0通信模型基于DDS的ROS2.0通信模型包含以下幾個(gè)關(guān)鍵概念。數(shù)據(jù)讀取器（DataReader）上層應(yīng)用從訂閱者讀取數(shù)據(jù)的對(duì)象，每個(gè)數(shù)據(jù)讀取器對(duì)應(yīng)一個(gè)特定的主題，類似于ROS1.0中的一個(gè)消息訂閱者。主題和ROS1.0中的概念類似，主題需要定義一個(gè)名稱和一種數(shù)據(jù)結(jié)構(gòu)，但ROS2.0中的每個(gè)主題都是一個(gè)實(shí)例，可以存儲(chǔ)該主題中的歷史消息數(shù)據(jù)。11.1ROS2.0設(shè)計(jì)思想3ROS2.0通信模型基于DDS的ROS2.0通信模型包含以下幾個(gè)關(guān)鍵概念。質(zhì)量服務(wù)（QualityofService）原則：簡(jiǎn)稱QoSPolicy，這是ROS2.0中新增的、也是非常重要的一個(gè)概念，主要從時(shí)間限制、可靠性、持續(xù)性、歷史記錄幾方面，滿足用戶針對(duì)不同場(chǎng)景的數(shù)據(jù)通信需求，如圖所示。11.1ROS2.0設(shè)計(jì)思想3ROS2.0通信模型從上面幾個(gè)重要概念，可以看出ROS2.0相比于ROS1.0，在以下幾方面有所提升。實(shí)時(shí)性增強(qiáng)：數(shù)據(jù)必須在截止日期之前完成更新。實(shí)時(shí)性增強(qiáng)：持續(xù)性增強(qiáng)：DDS可以為ROS2.0提供數(shù)據(jù)歷史服務(wù)，新加入的節(jié)點(diǎn)也可以獲取發(fā)布者發(fā)布的所有歷史數(shù)據(jù)。可靠性增強(qiáng)：配置可靠性原則，用戶可以根據(jù)需求選擇性能模式（BEST_EFFORT）或穩(wěn)定模式（RELIABLE）。11.2ROS2.0安裝與使用1ROS2.0安裝為使用ROS2.0，首先需要進(jìn)行編譯安裝。ROS2.0有多個(gè)版本，每個(gè)版本對(duì)應(yīng)的適配環(huán)境和功能不盡相同。本節(jié)以2020年6月發(fā)布的ROS2.0FoxyFitzroy版本為例，介紹ROS2.0的安裝流程。在安裝前，需要提前準(zhǔn)備好與之對(duì)應(yīng)的操作環(huán)境，可以使用虛擬機(jī)構(gòu)建Ubuntu20.04操作環(huán)境，具體構(gòu)建過程不再贅述。構(gòu)建好操作環(huán)境后，即可開始ROS2.0的安裝，流程如下：設(shè)置編碼，命令如下：11.2ROS2.0安裝與使用1ROS2.0安裝在安裝前，需要提前準(zhǔn)備好與之對(duì)應(yīng)的操作環(huán)境，可以使用虛擬機(jī)構(gòu)建Ubuntu20.04操作環(huán)境，具體構(gòu)建過程不再贅述。構(gòu)建好操作環(huán)境后，即可開始ROS2.0的安裝，流程如下：設(shè)置軟件源，命令如下：安裝ROS2.0（如果出現(xiàn)下載失敗問題，可以嘗試更換軟件源重新下載），命令如下：11.2ROS2.0安裝與使用1ROS2.0安裝在安裝前，需要提前準(zhǔn)備好與之對(duì)應(yīng)的操作環(huán)境，可以使用虛擬機(jī)構(gòu)建Ubuntu20.04操作環(huán)境，具體構(gòu)建過程不再贅述。構(gòu)建好操作環(huán)境后，即可開始ROS2.0的安裝，流程如下：安裝自動(dòng)補(bǔ)全工具，命令如下：安裝完ROS2.0后，需要進(jìn)行ROS2.0的環(huán)境變量更新，并將其加入用戶環(huán)境變量.bashrc文件中，以便在后續(xù)過程中更方便地進(jìn)行ROS2.0編程與仿真。命令如下：11.2ROS2.0安裝與使用2運(yùn)行小海龜案例本節(jié)將通過小海龜仿真案例，讓讀者初步體驗(yàn)ROS2.0。啟動(dòng)小海龜仿真器在命令行中輸入如下命令啟動(dòng)小海龜仿真器：命令執(zhí)行后會(huì)打開一個(gè)仿真器界面，里邊會(huì)隨機(jī)生成一只小海龜，如圖所示。在命令行中可以看到小海龜?shù)拿趾推湓诜抡嫫髯鴺?biāo)系下的位置（如果沒有小海龜仿真器功能包，可以通過命令sudoaptinstallros-foxy-turtlesim安裝）。11.2ROS2.0安裝與使用2運(yùn)行小海龜案例本節(jié)將通過小海龜仿真案例，讓讀者初步體驗(yàn)ROS2.0。啟動(dòng)小海龜仿真器命令行打印的小海龜位置信息如下：如圖所示，在新的命令行中可以通過上、下、左、右方向鍵分別控制小海龜?shù)那斑M(jìn)后退和旋轉(zhuǎn)方向，這與ROS1.0中是相同的。不同的是，ROS2.0多出了固定角度旋轉(zhuǎn)動(dòng)作（action）控制功能，可通過輸入G、B、V、C等鍵控制小海龜旋轉(zhuǎn)固定角度。11.2ROS2.0安裝與使用2運(yùn)行小海龜案例本節(jié)將通過小海龜仿真案例，讓讀者初步體驗(yàn)ROS2.0。分析消息的傳遞打開一個(gè)新的命令行，輸入如下命令：可以打開ROS節(jié)點(diǎn)圖形化界面。在左上角的下拉列表中選擇Nodes/Topics(all)選項(xiàng)，單擊刷新按鈕，可以看到小海龜案例對(duì)應(yīng)的節(jié)點(diǎn)、主題，以及帶箭頭的消息傳遞方向。其中，/turtlesim是圖形化小海龜節(jié)點(diǎn)，/teleop_turtle是小海龜運(yùn)動(dòng)控制節(jié)點(diǎn)，兩者通過主題/turtle1/cmd_vel進(jìn)行關(guān)聯(lián)，該主題為小海龜運(yùn)動(dòng)控制主題。11.2ROS2.0安裝與使用2運(yùn)行小海龜案例本節(jié)將通過小海龜仿真案例，讓讀者初步體驗(yàn)ROS2.0。分析消息的傳遞其余主題為控制小海龜固定角度旋轉(zhuǎn)動(dòng)作的主題，如圖所示。11.2ROS2.0安裝與使用2運(yùn)行小海龜案例本節(jié)將通過小海龜仿真案例，讓讀者初步體驗(yàn)ROS2.0。安裝rqt圖形化工具，生成新小海龜在Linux中通過如下命令安裝rqt圖形化工具：安裝完成后，通過命令rqt運(yùn)行rqt圖形化界面。第一次打開的界面是空的（如圖所示），選擇Plugins->Services->ServiceCaller選項(xiàng)，應(yīng)用用來發(fā)布服務(wù)請(qǐng)求的插件，單擊刷新按鈕之后，可以看到系統(tǒng)當(dāng)前的所有服務(wù)。11.2ROS2.0安裝與使用2運(yùn)行小海龜案例本節(jié)將通過小海龜仿真案例，讓讀者初步體驗(yàn)ROS2.0。安裝rqt圖形化工具，生成新小海龜選擇/spawn服務(wù)，雙擊下邊的數(shù)據(jù)項(xiàng)，填寫新產(chǎn)生小海龜?shù)拿郑╰urtle2）和位置，然后單擊Call按鈕，即可發(fā)送服務(wù)請(qǐng)求，生成一只新的小海龜。注意，名字不能和已有的小海龜名字一樣，否則會(huì)報(bào)錯(cuò)。請(qǐng)求服務(wù)后，產(chǎn)生的不僅是一只新的小海龜，還會(huì)有turtle2對(duì)應(yīng)的主題、服務(wù)，如圖所示。11.2ROS2.0安裝與使用2運(yùn)行小海龜案例本節(jié)將通過小海龜仿真案例，讓讀者初步體驗(yàn)ROS2.0。安裝rqt圖形化工具，生成新小海龜接下來控制第二只小海龜?shù)倪\(yùn)動(dòng)，仿真器默認(rèn)的鍵盤控制節(jié)點(diǎn)只能發(fā)布turtle1的主題，但是通過ROS的重映射（remapping）機(jī)制，可以實(shí)現(xiàn)對(duì)主題的重新命名。在一個(gè)新的命令行中輸入如下命令：11.2ROS2.0安裝與使用2運(yùn)行小海龜案例本節(jié)將通過小海龜仿真案例，讓讀者初步體驗(yàn)ROS2.0?？刂菩『｝斶\(yùn)動(dòng)打開一個(gè)新的命令行，輸入如下命令：此命令把turtle1/cmd_vel主題名更改為turtle2/cmd_vel，然后就可以通過該節(jié)點(diǎn)控制turtle2小海龜?shù)倪\(yùn)動(dòng)了。11.2ROS2.0安裝與使用3ROS2.0命令行ROS2.0的主要命令行入口為ros2，使用命令“ros2<command>”進(jìn)行調(diào)用。相關(guān)命令介紹如下：action動(dòng)作相關(guān)的子命令bagrosbag相關(guān)的子命令component組件相關(guān)子命令daemon守護(hù)進(jìn)程相關(guān)的子命令doctor檢查ROS設(shè)置和其他潛在問題interface顯示ROS接口的信息11.2ROS2.0安裝與使用3ROS2.0命令行ROS2.0命令行與ROS1.0命令行在功能上有很大的相似之處。在以上命令中，需要注意以下經(jīng)常使用到的命令：ros2node：輸出節(jié)點(diǎn)相關(guān)信息。ros2run：運(yùn)行一個(gè)功能包節(jié)點(diǎn)/可執(zhí)行文件，是ROS2.0中的基本運(yùn)行命令。ros2topic：操作主題信息，如使用ros2topiclist可以查看當(dāng)前運(yùn)行的所有主題名，使用ros2topicinfo可以打印主題相關(guān)信息，使用ros2topicpub可以發(fā)布新主題。ros2param：操作參數(shù)服務(wù)器信息，如增加參數(shù)、修改參數(shù)等。11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法ROS2.0的編程方法與ROS1.0類似，主要包括三種通信機(jī)制：主題通信、服務(wù)通信、參數(shù)服務(wù)器通信。這三種通信機(jī)制的基本原理與思想與ROS1.0并無差別，只是實(shí)現(xiàn)上有所不同。為了讓讀者更清晰地理解ROS2.0編程細(xì)節(jié)，本節(jié)通過主題通信案例——?jiǎng)?chuàng)建ROS2.0工作空間、創(chuàng)建ROS2.0功能包、創(chuàng)建一個(gè)簡(jiǎn)單的訂閱者和發(fā)布者（C++），來介紹ROS2.0編程方法。更多有關(guān)ROS2.0與ROS1.0編程的區(qū)別，將在下一節(jié)進(jìn)行介紹。11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：設(shè)置ROS2.0的環(huán)境變量，命令如下：工作空間的層次是通過環(huán)境變量來配置的。簡(jiǎn)單來說，下一個(gè)配置的工作空間會(huì)放到上一個(gè)配置的工作空間之上。ROS2.0安裝路徑下的功能包一般會(huì)被設(shè)置為最下層的工作空間。11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建一個(gè)新的文件夾，命令如下：其中dev_ws是創(chuàng)建的工作空間，src是放置功能包相關(guān)文件的位置。11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：編譯工作空間，編譯工作空間需要在工作空間的根目錄dev_ws下執(zhí)行，命令如下：如果提示colcon沒有安裝，可以使用如下命令安裝：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：colcon是ROS2.0的編譯工具，類似于ROS1.0中的catkin，該命令尾部還可以緊跟一些常用的參數(shù)后綴，如：--packages-up-to：編譯指定的功能包，而不是整個(gè)工作空間；--symlink-install：節(jié)省每次重建Python腳本的時(shí)間；event-handlersconsole_direct+：在命令行中顯示編譯過程中的詳細(xì)日志。11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建ROS2.0功能包功能包是ROS2.0中組織代碼的基本容器，方便編譯、安裝和開發(fā)。一般來講，每個(gè)功能包都是用來完成某項(xiàng)具體的功能相對(duì)完整的單元。ROS2.0中的功能包可以使用CMake或Python兩種方式來編譯，其本身是一個(gè)“文件夾”，但和文件夾不同的是，每個(gè)功能包中至少都會(huì)有兩個(gè)文件：package.xml：功能包的描述信息。CMakeLists.txt：描述CMake編譯該功能包的規(guī)則。11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建ROS2.0功能包每個(gè)工作空間中可以有多個(gè)功能包，但是功能包不能嵌套。一個(gè)工作空間內(nèi)典型的功能包結(jié)構(gòu)如下：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建ROS2.0功能包創(chuàng)建功能包需要在工作空間目錄下進(jìn)行，使用之前已經(jīng)創(chuàng)建好的dev_ws工作空間，在其中的src文件夾中，創(chuàng)建新功能包，命令如下：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建ROS2.0功能包命令執(zhí)行后，將創(chuàng)建一個(gè)名為<package_name>的功能包，該功能包目錄下有include文件夾、src文件夾、CMakeLists.txt文件和package.xml文件。其中include文件夾存放用戶編寫的庫(kù)文件，src文件夾存放.cpp文件。創(chuàng)建功能包的命令還允許設(shè)置節(jié)點(diǎn)名，并自動(dòng)生成一個(gè)名為helloworld的例程代碼。如下述命令將生成一個(gè)名為my_package的功能包及該功能包下名為my_node的節(jié)點(diǎn)，該節(jié)點(diǎn)中存在一個(gè)自動(dòng)生成的helloworld的例程代碼：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建ROS2.0功能包上述命令創(chuàng)建的都是CMake功能包，若要?jiǎng)?chuàng)建Python功能包，則將ament_cmake更改為ament_python即可。需要注意的是，每次修改功能包后，都需要重新編譯工作空間，如果只想編譯某個(gè)特定的功能包，可以使用如下命令，其中my_package是指定編譯的功能包名。11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建ROS2.0功能包完成工作空間和功能包創(chuàng)建后，將得到如下所示的文件樹結(jié)構(gòu)：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建一個(gè)簡(jiǎn)單的訂閱節(jié)點(diǎn)和發(fā)布節(jié)點(diǎn)（C++）創(chuàng)建發(fā)布節(jié)點(diǎn)，進(jìn)入my_package/src目錄下，創(chuàng)建HelloWorld_Pub.cpp文件，并將以下代碼復(fù)制進(jìn)去：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建一個(gè)簡(jiǎn)單的訂閱節(jié)點(diǎn)和發(fā)布節(jié)點(diǎn)（C++）創(chuàng)建發(fā)布節(jié)點(diǎn)，進(jìn)入my_package/src目錄下，創(chuàng)建HelloWorld_Pub.cpp文件，并將以下代碼復(fù)制進(jìn)去：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建一個(gè)簡(jiǎn)單的訂閱節(jié)點(diǎn)和發(fā)布節(jié)點(diǎn)（C++）創(chuàng)建訂閱節(jié)點(diǎn)，同樣在my_package/src目錄下，創(chuàng)建HelloWorld_Sub.cpp文件，并將以下代碼復(fù)制進(jìn)去：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建一個(gè)簡(jiǎn)單的訂閱節(jié)點(diǎn)和發(fā)布節(jié)點(diǎn)（C++）創(chuàng)建訂閱節(jié)點(diǎn)，同樣在my_package/src目錄下，創(chuàng)建HelloWorld_Sub.cpp文件，并將以下代碼復(fù)制進(jìn)去：11.3ROS2.0編程基礎(chǔ)1ROS2.0編程方法創(chuàng)建ROS2.0工作空間創(chuàng)建ROS2.0工作空間的流程如下：創(chuàng)建一個(gè)簡(jiǎn)單的訂閱節(jié)點(diǎn)和發(fā)布節(jié)點(diǎn)（C++）設(shè)置依賴項(xiàng)與編譯規(guī)則。首先打開功能包的package.xml文件，把標(biāo)記有TODO的內(nèi)容填寫完整：添加所需依賴項(xiàng)，放在ament_cmake下邊：11.3R