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

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

，

，

，

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

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

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

、

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

，可得：式中，

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

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

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

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

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

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

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

和平移向量

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

，并記

，

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

，

，

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