機器人操作系統(tǒng)ROS應用實踐 課件 第9、10章 基于視覺的機械臂抓取、移動機器人視覺SLAM

第九章基于視覺的機械臂抓取機器人操作系統(tǒng)ROS應用實踐目錄content深度相機9.19.5基于深度學習的物體識別9.2本章小結手眼標定原理和過程9.3基于視覺的機械臂抓取實現(xiàn)9.49.1深度相機1雙目相機和RGB-D深度相機雙目相機雙目立體視覺算法是模擬人眼系統(tǒng)的一種算法，因低廉的成本、較快的速度得到廣泛使用。該算法用雙目模擬人眼，基于視差原理，利用成像設備從不同的角度、位置對目標物體進行觀察，實時采集左右兩個圖像，利用匹配算法實現(xiàn)左右圖像特征點對的匹配，進而得到視差圖，然后利用三角幾何原理，實現(xiàn)對目標物體的三維信息恢復。9.1深度相機1雙目相機和RGB-D深度相機雙目相機采用雙目立體視覺算法獲取深度圖像的設備稱為雙目相機。雙目相機的主要優(yōu)點如下。對硬件要求低，成本也低。對室內外場景的適應性好。9.1深度相機1雙目相機和RGB-D深度相機雙目相機采用雙目立體視覺算法獲取深度圖像的設備稱為雙目相機。但是雙目相機的缺點也非常明顯，具體如下。對環(huán)境光照敏感，光線變化易導致匹配失敗或精度降低。計算復雜度較高，涉及左右兩個攝像頭的特征匹配，需要消耗較多的計算資源?；€（兩個攝像頭間距）限制了測量范圍，測量范圍和基線成正比。不適用于缺乏紋理的場景（如白墻場景），因為雙目視覺算法根據(jù)視覺特征進行圖像匹配，沒有特征會導致匹配失敗。9.1深度相機1雙目相機和RGB-D深度相機RGB-D深度相機RGB-D深度相機通過結構光或ToF測量物體深度信息，相比于雙目相機通過視差計算深度的方式，RGB-D深度相機能夠主動測量每個像素的深度。RGB-D深度相機最大的特點是不僅能夠像普通相機一樣去獲取圖像信息，還能夠得到深度圖，深度圖中包含每個像素點的深度數(shù)據(jù)（即空間中的點到相機成像平面的距離），避免了雙目相機需要利用視差，通過軟件計算每個像素點深度信息的煩瑣過程。RGB-D深度相機除含有一個普通的光學攝像頭外，還含有一個發(fā)射器和一個接收器。RGB-D深度相機主動向物體發(fā)射并接收返回光，通過物理手段計算出相機與物體之間的距離。9.1深度相機1雙目相機和RGB-D深度相機RGB-D深度相機如圖所示，微軟公司2012年發(fā)布的KinectV1相機是一種基于結構光的RGB-D深度相機，中間的元件為RGB攝像頭（ColorCamera），可采集彩色圖像；左邊的元件為紅外發(fā)射器（IRProject），右邊的元件為紅外接收器（IRCamera），左右兩個元件組成了深度傳感器（DepthSensor）。9.1深度相機1雙目相機和RGB-D深度相機RGB-D深度相機基于結構光的RGB-D深度相機的基本原理是：通過紅外發(fā)射器，將具有一定結構特征的紅外線圖案（Pattern）投影到物體表面，再由專門的攝像頭進行采集。這種具備一定圖案結構的光線到達被攝物體表面時，被攝物體表面結構的不同會導致圖案結構變形，通過運算單元可將這種圖案結構變形換算成深度信息，以此來獲得被攝物體的三維結構。根據(jù)結構光圖案的不同，其一般可分為線掃描結構光（也叫條紋結構光）和面陣結構光。其中，線掃描結構光比面陣結構光簡單，精度也比較高，在工業(yè)中廣泛用于物體體積測量、三維成像等領域。面陣結構光大致可分為兩類：編碼結構光和隨機結構光。9.1深度相機1雙目相機和RGB-D深度相機RGB-D深度相機編碼結構光又可以分為時序編碼結構光和空間編碼結構光。時序編碼結構光是在一定時間范圍內，通過投影器向被測空間投射的一系列明暗不同的結構光。每次投射都通過相機進行成像，對每個像素生成唯一的編碼值。時序編碼結構光的優(yōu)點是精度高，缺點是只適用于靜態(tài)場景，而且需要拍攝大量圖像，實時性不好。為滿足動態(tài)場景的需要，可以采用空間編碼結構光。9.1深度相機1雙目相機和RGB-D深度相機RGB-D深度相機編碼結構光又可以分為時序編碼結構光和空間編碼結構光?？臻g編碼結構光特指向被測空間中投影的經過數(shù)學編碼的、一定范圍內的、光斑不具備重復性的結構光。這樣，某個點的編碼值可以通過其鄰域獲得。重建的精度取決于空間編碼的像素數(shù)量（窗口大小）。常用的空間編碼方式有方波、正弦波、德布魯因序列、二維空間編碼。空間編碼結構光的優(yōu)點是無須很多圖像，只需要一對圖像即可進行三維重建，實時性好，常用于動態(tài)環(huán)境中；缺點是易受噪聲干擾，反光、照明等原因可能導致成像時部分區(qū)域的編碼信息缺失，對空間中的遮擋比較敏感，相較于時序編碼結構光精度較低。9.1深度相機1雙目相機和RGB-D深度相機RGB-D深度相機隨機結構光較為簡單，也更加常用。隨機結構光是投影器向被測空間中投射的亮度不均和隨機分布的點狀結構光（不帶編碼信息）。隨機結構光通過雙目相機成像，所得的圖像經過極線校正后再進行雙目稠密匹配（與雙目算法相似），即可重建出對應的深度圖。由于結構光容易受環(huán)境光干擾，在室外場景中精度差，同時隨著檢測距離的增加，其精度也會變差，只適合中短距離使用。因此，出現(xiàn)了結構光與雙目立體視覺融合的技術，采用這種技術的深度相機包括圖漾雙目深度相機、IntelRealSenseD4系列雙目深度相機等。9.1深度相機2雙目相機模型和RGB-D深度相機模型雙目相機模型雙目相機測距的原理與人眼類似，通過對兩圖像視差的計算，實現(xiàn)對兩圖像中同一物點距離的測量。雙目立體視覺測距模型如圖所示，模型假設左右兩個相機的光軸平行，成像平面共面，且焦距等內在參數(shù)一致。9.1深度相機2雙目相機模型和RGB-D深度相機模型雙目相機模型設相機的焦距為f、相機間的基線長度（左右相機光心之間的距離）為b，根據(jù)幾何關系，可以列出以下方程：xl和xr為P點在左右兩個成像平面的投影點在X方向上的坐標，yl和yr為P點在左右兩個成像平面的投影點在Y方向上的坐標。進而可解得：9.1深度相機1雙目相機模型和RGB-D深度相機模型雙目相機模型定義視差

，即左成像平面上的點

和右成像平面上的對應點

的橫坐標之差，則有：通過上式，能夠得到相機坐標系與圖像坐標系之間的變換關系?？梢姡绻老鄼C的焦距、兩相機間基線長度和視差，就可以計算點P的深度z。焦距及基線長度可通過相機標定獲得，而視差的計算則是雙目立體視覺需要解決的核心問題，一般通過對左右相機采集到的兩圖像進行特征點匹配來實現(xiàn)。9.1深度相機2雙目相機模型和RGB-D深度相機模型RGB-D深度相機模型KinectV2相機通過ToF法測量相機與目標物體之間的距離，而ToF法利用紅外線在空氣中傳播的時間，通過紅外發(fā)射器向物體發(fā)射紅外線，然后記錄紅外線從發(fā)射到返回之間的時間，再通過計算確定相機與物體之間的距離，原理如圖所示。9.1深度相機2雙目相機模型和RGB-D深度相機模型RGB-D深度相機模型對于RGB-D深度相機，需要求解深度相機與2D彩色相機之間的外參矩陣，將RGB圖像與深度圖像對齊，雙相機幾何關系如圖所示。9.1深度相機2雙目相機模型和RGB-D深度相機模型RGB-D深度相機模型假設物體表面上有一點P，它在深度圖像上的投影坐標為pir，在深度相機坐標系下的坐標為Pir，則有：式中，Hir為深度相機的內參矩陣。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別MaskR-CNNMaskR-CNN（MaskRegion-ConvolutionNeuralNetwork，帶掩碼的區(qū)域卷積神經網(wǎng)絡）是目前流行的實例分割架構，能夠對任意大小的輸入圖像進行特征學習。MaskR-CNN擴展自FasterR-CNN，由何凱明在2017年提出。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別MaskR-CNNFasterR-CNN是一個流行的目標檢測框架，MaskR-CNN在FasterR-CNN的基礎上，將RoIPooling（RegionofInterestPooling，感興趣區(qū)域池化）層替換成了RoIAlign（區(qū)域特征聚集）層，并添加了一個預測分割掩碼（Mask）的分支（FCN層，即全連接層），將其擴展為實例分割框架。MaskR-CNN網(wǎng)絡結構如圖所示。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別SSDSSD（SingleShotMultiBoxDetector，單步多框目標檢測）是基于Caffe框架實現(xiàn)的一種目標檢測算法，其特點是識別速度快且準確率高。SSD網(wǎng)絡結構如圖所示。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別SSDSSD網(wǎng)絡的主體結構是在一個基礎網(wǎng)絡的基礎上，增加一系列小的卷積神經網(wǎng)絡對目標位置與目標所屬種類的信任程度進行預測。SSD在用于圖像分類的基礎網(wǎng)絡上，增加若干不同尺度的特征映射圖，它們的尺度逐漸降低，用于獲得不同大小的感受野（ReceptiveField，RF）。尺度較大的特征映射圖的感受野較小，可以對小的目標進行感知，尺度較小的特征映射圖的感受野較大，可以對較大的目標進行感知。SSD通過多種尺度的特征映射圖，對多種尺度的目標進行感知。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別SSD與傳統(tǒng)的卷積神經網(wǎng)絡結構相比，SSD網(wǎng)絡修改了輸入層以用于數(shù)據(jù)擴充，除輸入圖像信息之外，其還通過一個基礎網(wǎng)絡獲得圖像的高層次特征（目標框的位置及形狀信息），并在其后擴展出若干不同尺度的特征映射圖。在每張?zhí)卣饔成鋱D的后面，分別添加一個小的卷積神經網(wǎng)絡作為特征檢測器，稱為CNN特征檢測器（CNNDetector），對目標的位置及其所屬種類做出識別，最后經過損失層整合，進行非極大值抑制后輸出。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別SSD對于SSD網(wǎng)絡而言，在訓練階段，有兩個輸入：待提取目標的圖像，以及目標的位置、大小和形狀信息；在識別階段，則只需要輸入圖像即可。除此之外，為了使訓練出的網(wǎng)絡能夠具有更好的泛化能力（即在目標尺寸較小、發(fā)生形變、部分遮擋等情況出現(xiàn)時，依舊能夠很好第識別目標），需要在原始訓練數(shù)據(jù)的基礎上進行數(shù)據(jù)擴充，豐富訓練集樣本的種類。SSD網(wǎng)絡中采用的數(shù)據(jù)擴充方式主要有隨機采樣、圖像變換兩種。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別SSDCNN特征檢測器的組成如圖所示，每個CNN特征檢測器由三部分組成：先驗框生成器（PriorBoxGenerator）、位置回歸器（CNNforPredicting（x,y,w,h））及種類置信度回歸器（CNNforPredictingConfidence）。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別SSD對于一個卷積神經網(wǎng)絡，損失的計算方式是由訓練目標決定的，即期望輸出與網(wǎng)絡輸出的差值。在訓練階段，損失層計算得到損失以后，進行反向傳播和權值修正。在SSD網(wǎng)絡中，損失層共有四個輸入，分別為網(wǎng)絡輸入中的標注信息、先驗框生成器生成的先驗框、位置回歸器得到的位置信息、種類置信度回歸器得到的各個類別的置信度。在損失層，將根據(jù)這四的組成輸入計算損失并進行反向傳播，同時發(fā)掘具有代表性的負樣本用于網(wǎng)絡的進一步訓練。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別YOLOYOLO（YouOnlyLookOnce）是RedmonJ在2015年提出的一種全新的端到端的目標檢測算法。在檢測過程中，YOLO將目標檢測算法的生成目標候選區(qū)域、提取檢測目標特征、檢測目標候選驗證的三個階段，用一個神經網(wǎng)絡加以整合，直接快速地完成目標檢測任務。YOLO提取整個圖像的特征信息，分析每個邊界框的坐標參數(shù)和其中的目標所屬類別的概率，將目標檢測分為邊界框坐標參數(shù)的回歸問題和類別的分類問題來處理，從而得到目標所屬類別及其位置坐標。YOLO算法在保證較高平均精度的同時，實現(xiàn)了端到端的訓練和實時識別，標準的YOLO算法每秒能夠檢測45個圖像，F(xiàn)astYOLO算法每秒能夠檢測155個圖像。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別YOLOYOLO目標檢測流程如圖所示。首先將輸入的圖像劃分為

個網(wǎng)格（GridCell），如果圖像中某一個目標的中心剛好落在其中的某一個網(wǎng)格中，那么這個目標就由該網(wǎng)格進行預測。9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別YOLO每個網(wǎng)格預測B個邊界框及其置信度評分，每個預測邊界框包含5個預測值：x、y、w、h和置信度（Confidence）。(x,y)坐標表示邊界框的中心相對其網(wǎng)格的位置，(w,h)為邊界框相對于整個圖像的寬和高。置信度反映了所預測的邊界框內是否含有目標，以及邊界框預測的準確度，其計算公式為：9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別YOLO如果網(wǎng)格中不存在目標，則式中的

，即置信度也為0。如果存在某一目標的中心落在網(wǎng)格中，則

，置信度等于預測目標框面積與真實目標框面積之間的交集與并集之比（IntersectionoverUnion，IOU），計算公式為：9.2基于深度學習的物體識別1基于卷積神經網(wǎng)絡的物體識別YOLO同時，每個單元網(wǎng)格還在網(wǎng)格內存在目標的條件下，對目標是C個類別中的某一類的概率進行預測，得到C個類別概率

，表示該網(wǎng)格中存在第i類物體中心的概率。C為類別數(shù)量，與B無關。將類別預測得到的值與預測邊界框的置信度相乘，可得到每個預測邊界框在其網(wǎng)格中的某個類別物體i的置信度Confidence(i)，即：9.2基于深度學習的物體識別1常見深度學習框架TensorFlow框架TensorFlow是Google提出的深度學習框架，具有Python和C++接口，利用其靈活易用的前端語言，人們可以輕松地構建復雜的算法模型；同時，其后端高效的執(zhí)行系統(tǒng)與分布式架構保證了在模型訓練和執(zhí)行方面的高性能。TensorFlow從2015年11月開源以來，一直是GitHub機器學習門類中最受關注的項目之一，也是業(yè)界優(yōu)秀的深度學習框架之一。9.2基于深度學習的物體識別1常見深度學習框架TensorFlow框架TensorFlow是一個基于數(shù)據(jù)流圖（DataFlowGraph）的數(shù)值計算開源軟件庫，其靈活的架構設計允許開發(fā)者以單機或分布式的方式，將計算部署到臺式機或服務器上。數(shù)據(jù)流圖是TensorFlow中對計算過程的抽象描述，如圖所示。數(shù)據(jù)流圖是有向圖，圖中的各節(jié)點表示各種數(shù)學計算操作，每個邊表示操作與操作之間傳遞的高維數(shù)組數(shù)據(jù)，稱為tensor。9.2基于深度學習的物體識別1常見深度學習框架TensorFlow框架TensorFlow作為一個深度學習框架，主要具有以下優(yōu)勢?；赥ensorFlow搭建模型時使用的代碼量少，可以用非常簡潔的語言實現(xiàn)各種復雜的算法模型，將開發(fā)者從非常消耗精力的編碼和調試工作中解放出來，在提高程序可維護性的同時節(jié)省大量的開發(fā)時間。TensorFlow的內核執(zhí)行系統(tǒng)使用C++語言編寫，擁有非常高的執(zhí)行效率。9.2基于深度學習的物體識別1常見深度學習框架TensorFlow框架TensorFlow作為一個深度學習框架，主要具有以下優(yōu)勢。TensorFlow具有優(yōu)秀的分層架構設計，采用前端編程語言與后端執(zhí)行引擎分離，執(zhí)行引擎與面向硬件的計算分離的架構，使模型非常方便地運行在異構設備環(huán)境中，充分實現(xiàn)了“高內聚，低耦合”的設計思想，開發(fā)者在前端可以不用關心硬件運行的特殊機制，而專心于算法和應用方面的研究。9.2基于深度學習的物體識別1常見深度學習框架PyTorch框架PyTorch的前身是Torch。自從互聯(lián)網(wǎng)巨頭原Facebook人工智能研究院（FAIR）開源了大量基于Torch框架的深度學習模塊后，Torch逐漸流行起來，它是一個擁有大量機器學習算法支撐的科學計算框架，應用靈活。Torch框架的一個特殊之處是采用Lua作為編程語言，后來PyTorch框架使用Python語言重寫了Torch框架底層很多內容，不僅更加靈活，支持動態(tài)圖，還提供了Python接口。9.2基于深度學習的物體識別1常見深度學習框架PyTorch框架PyTorch具有以下優(yōu)點。TensorFlow與Caffe都采用命令式的編程語言，而且是靜態(tài)的，使用TensorFlow時，首先必須構建一個神經網(wǎng)絡，如果想要改變網(wǎng)絡結構，需要從頭開始。但是PyTorch是動態(tài)的，通過一次反向求導的技術，開發(fā)者可以任意改變神經網(wǎng)絡的行為。到目前為止，PyTorch的實現(xiàn)是最快的，可以獲得很高的靈活性，這也是PyTorch對比TensorFlow具有的最大的優(yōu)勢。9.2基于深度學習的物體識別1常見深度學習框架PyTorch框架PyTorch具有以下優(yōu)點。PyTorch的設計思路是線性、直觀且易于實現(xiàn)的，當代碼出現(xiàn)問題時，開發(fā)者可以輕松地找到出錯的代碼，不會因錯誤的指向浪費太多的調試時間。PyTorch的代碼相對于TensorFlow而言，更加簡潔、直觀。對比TensorFlow高度工業(yè)化的底層代碼，PyTorch的源碼更友好，更容易看懂，開發(fā)者對其框架的理解會更深。9.2基于深度學習的物體識別1常見深度學習框架PyTorch框架PyTorch也存在以下缺點。針對移動端、嵌入式部署及高性能服務器端的部署，性能表現(xiàn)有待提升。由于PyTorch的流行比TensorFlow晚一些，它的開發(fā)社區(qū)沒有那么強大，實現(xiàn)的相關開源代碼較少，不支持某些運算（如快速傅里葉變換、檢查無窮與非數(shù)值張量等）。9.3手眼標定原理和過程為了使機器人能夠根據(jù)相機采集的圖像信息進行操作，需要獲得相機（機器人的眼）與機器人末端夾爪（機器人的手，以下簡稱機器人末端）之間的坐標變換關系，即對相機坐標系與機器人末端坐標系進行標定，該過程也叫手眼標定。通過第8章的相機標定，我們得到了相機的內外參數(shù)和畸變系數(shù)，建立了物體坐標系到像素坐標系之間的變換。而通過機械臂夾爪去抓取物體，需要建立機械臂與目標物體之間的坐標變換關系，相機可以作為聯(lián)系兩者的“橋梁”，問題就可以轉化為求機械臂與相機之間的坐標變換關系。因此，通過手眼標定，獲取機器人末端坐標系和相機坐標系之間的關系，就可以將識別的目標物體位置坐標變換到機器人末端坐標系下，進而實現(xiàn)物體抓取等操作。9.3手眼標定原理和過程機器人的手眼關系分為eye-in-hand及eye-to-hand兩種，如圖所示。其中eye-in-hand叫眼在手上，機器人的視覺系統(tǒng)隨著機器人末端運動；而eye-to-hand叫眼在手外，機器人的視覺系統(tǒng)不會在世界坐標系內運動，與機器人基座的坐標變換關系是固定的。對于eye-in-hand的情況，機器人手眼標定即標定機器人末端與相機之間的坐標變換關系；對于eye-to-hand的情況，機器人手眼標定即標定機器人基座與相機之間的坐標變換關系。9.3手眼標定原理和過程兩種手眼標定方式的基本原理相似，如圖所示，下面介紹標定原理。設

為機器人末端坐標系相對于機器人基座坐標系的變換矩陣，

為相機坐標系相對于機器人末端坐標系的變換矩陣，則相機坐標系相對于機器人基座坐標系的變換矩陣為其中，

可利用機器人各關節(jié)角度及正運動學方程計算得到，而

需要通過手眼標定算法來求解。9.3手眼標定原理和過程又設

為物體相對于相機坐標系的變換矩陣，則：在eye-in-hand方式中，機器人運動到任意兩個位姿，有以下公式：令可得：在eye-to-hand方式中，機器人末端夾著標定板，運動到任意兩個位姿，有以下公式：令可得：9.3手眼標定原理和過程為進一步求解齊次方程

，先將齊次變換矩陣寫成旋轉矩陣和平移向量的形式：將上式展開，即可得到待求解的方程組：9.3手眼標定原理和過程在上面的方程組中，RA、RB、tA、tB均可通過測量得到，I為單位陣。此方程組有多種解法，其中最經典的解法為Tsai-Lenz兩步法，即先利用方程的上式求解出旋轉矩陣R，再代入下式求解出平移向量t。求解過程中，利用旋轉向量-旋轉角來描述旋轉變換，具體的實現(xiàn)步驟如下。將旋轉矩陣RA、RB變換為旋轉向量rA、rB。根據(jù)羅德里格斯旋轉公式（Rodrigues’RotationFormula），旋轉矩陣R*可以通過旋轉向量

和旋轉角

進行表示：式中，

為r的反對稱陣：9.3手眼標定原理和過程在上面的方程組中，RA、RB、tA、tB均可通過測量得到，I為單位陣。此方程組有多種解法，其中最經典的解法為Tsai-Lenz兩步法，即先利用方程的上式求解出旋轉矩陣R，再代入下式求解出平移向量t。求解過程中，利用旋轉向量-旋轉角來描述旋轉變換，具體的實現(xiàn)步驟如下。定義新的旋轉向量表達Pr，有：式中，

為旋轉向量r的單位向量：進而可得：9.3手眼標定原理和過程在上面的方程組中，RA、RB、tA、tB均可通過測量得到，I為單位陣。此方程組有多種解法，其中最經典的解法為Tsai-Lenz兩步法，即先利用方程的上式求解出旋轉矩陣R，再代入下式求解出平移向量t。求解過程中，利用旋轉向量-旋轉角來描述旋轉變換，具體的實現(xiàn)步驟如下。根據(jù)下式，計算初始旋轉向量

：根據(jù)下式，計算旋轉向量P：9.3手眼標定原理和過程在上面的方程組中，RA、RB、tA、tB均可通過測量得到，I為單位陣。此方程組有多種解法，其中最經典的解法為Tsai-Lenz兩步法，即先利用方程的上式求解出旋轉矩陣R，再代入下式求解出平移向量t。求解過程中，利用旋轉向量-旋轉角來描述旋轉變換，具體的實現(xiàn)步驟如下。根據(jù)下式，計算旋轉矩陣R：根據(jù)計算所得旋轉矩陣R，求解平移向量t：9.3手眼標定原理和過程因此，可以將手眼標定的具體過程分為下面四個步驟進行。對相機進行標定，獲得相機內參。使用eye-to-hand或者eye-in-hand方式將相機、標定板固定好，啟動機器人，調整機器人末端位姿，并將對應的照片、機器人末端位姿記錄下來。利用相機內參，計算得到照片中相機與標定板之間的坐標變換關系。利用Tsai-Lenz兩步法求解前述方程，先從方程中求解出旋轉矩陣，再求解出平移向量，得到4×4

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

，

，

，

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

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

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

、

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

，可得：式中，

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

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

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

及相機內參矩陣K，則可求得比例因子

：10.3稠密建圖2雙目相機幾何模型與標定雙目相機外參標定設相機坐標系到世界坐標系的旋轉矩陣

，根據(jù)旋轉矩陣的性質有

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

和平移向量

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

，并記

，

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

，

，

，可得：進一步可得：10.3稠密建圖2雙目相機幾何模型與標定3D-2D映射：PnP從P3P原理中可以看出，為了求解PnP，需利用三角形相似的性質，求解投影點a、b、c在相機坐標系下的3D坐標，從而把問題轉換成一個3D到3D的位姿估計問題。然而，P3P也存在以下問題。P3P只利用三個點的信息，當給定的配對點多于三對時，其難以利用更多的信息。如果3D點或2D點受噪聲影響，或者存在誤匹配，則算法失效。10.3稠密建圖3稠密建圖基于單目相機的稠密三維重建難點在于快速實時恢復稠密的深度圖像，完成這一過程需要進行大量的計算，算法需要在效率和精度之間保持平衡。通常選取兩幀具有一定視差的圖像，通過立體視覺算法快速計算每個像素的深度值。一個標準的方法是為第一幀圖像的每一個像素x1在第二幀圖像對應的極線上搜索最相似的像素x2，這個過程稱為極線搜索。在搜索過程中，通過比較像素周圍局部圖像塊A,B的相似度來確定最佳匹配。10.3稠密建圖3稠密建圖常用圖像塊差異性或相似度的度量方法如下。SAD（SumofAbsoluteDifference），即用圖像塊每個像素灰度差的絕對值和度量：SSD（SumofSquaredDistance），即用圖像塊每個像素灰度差的平方和度量：NCC（NormalizedCrossCorrelation），即用圖像塊之間的相關性度量：10.3稠密建圖3稠密建圖綜上所述，單目稠密建圖的深度估計完整過程如下：假設圖像上的像素深度整體上滿足均勻－高斯混合分布。通過極線搜索和塊匹配確定重投影像素的位置。根據(jù)搜索結果三角化出每個像素的深度值并計算不確定性。將恢復的深度圖像與上一次估計相融合。若收斂，則結束算法，否則返回步驟（2）。10.3稠密建圖3稠密建圖KinectFusion稠密建圖算法的流程如下。深度圖像轉換：將獲得的深度圖像轉換成點云，根據(jù)相鄰像素點求得每個點的法向量。相機跟蹤：計算全局模型在上一幀相機位姿下的深度圖像，將該深度圖像和當前幀深度圖像都轉換成點云，通過ICP（IterativeClosestPoint，迭代最近點）算法，配準求得兩幀間的相對運動。體素化表示：采用截斷符號距離函數(shù)（TruncatedSignedDistanceFunction，TSDF）表示三維模型，不斷融合深度數(shù)據(jù)，將待重建的三維空間均勻劃分成網(wǎng)格，每個體素中存儲其中心到最近的三維物體表面的距離。經步驟（2）后，將當前幀點云融合到三維模型中。10.3稠密建圖3稠密建圖KinectFusion稠密建圖算法的流程如下。光線投射：計算在當前視角下場景的可見表面，用來和下一幀圖像進行配準，計算下一幀相機的位姿。注意：在步驟（3）中，KinectFusion稠密建圖算法采用GPU更新網(wǎng)格中的值，只要顯卡滿足重建的幀率不低于深度相機的幀率，就能做到實時稠密三維重建。10.3稠密建圖3稠密建圖但KinectFusion稠密建圖算法存在以下局限性。使用ICP算法進行相機位姿跟蹤，十分依賴場景的紋理豐富程度?；贙inect相機，預設TSDF中的三維網(wǎng)格，限制了場景尺度。沒有提供類似ORB-SLAM算法的回環(huán)檢測。基于這些問題，人們后來對KinectFusion稠密建圖算法進行了改進。例如，為減少對紋理的依賴性，融合ICP和RGB-D深度相機跟蹤方法；為突破場景尺度限制，令三維網(wǎng)格隨相機運動，讓三維網(wǎng)格外的區(qū)域不參與計算；采用DBoW算法進行回環(huán)檢測，提高三維稠密建圖的準確性。10.4其他視覺SLAM算法或框架1LSD-SLAM基于圖像特征識別的視覺SLAM屬于特征點法，特征點法又稱為兩視圖幾何（Two-ViewGeometry）法，該算法通過提取圖像的特征點，對前后幀圖像的特征點進行匹配，進而恢復相機的位姿。特征點法較為成熟，能夠獲得較精確的位姿估計。在處理輸入圖像時，首先提取圖像中的特征點，然后在求解過程中，最小化特征點的幾何位姿誤差。特征點法會丟失圖像中許多有效信息，特別是在弱紋理場景中，特征點法無法提取有效的特征點，將失效。10.4其他視覺SLAM算法或框架1LSD-SLAMLSD-SLAM（LargeScaleDirectmonocularSLAM）與DSO（DirectSparseOdometry）屬于直接法視覺SLAM，能夠實時構建稠密