機(jī)器人SLAM技術(shù)及其ROS系統(tǒng)應(yīng)用 第2版 課件 第4、5章 基于特征估計的激光SLAM技術(shù)、基于視覺的SLAM技術(shù)

基于特征估計的激光SLAM技術(shù)第四章目錄CONTENTS01rviz和Gazebo工具02激光雷達(dá)傳感器03基于激光的Gmapping算法04基于激光的HectorSLAM算法05基于激光的Cartographer算法01rviz和Gazebo工具

機(jī)器人系統(tǒng)中存在大量數(shù)據(jù)，這些數(shù)據(jù)在計算過程中往往都處于數(shù)據(jù)形態(tài)，比如圖像數(shù)據(jù)中0~255的RGB值。但是這種數(shù)據(jù)形態(tài)的值往往不利于開發(fā)者去感受數(shù)據(jù)所描述的內(nèi)容，所以常常需要將數(shù)據(jù)可視化顯示，例如機(jī)器人模型的可視化、圖像數(shù)據(jù)的可視化、地圖數(shù)據(jù)的可視化等。rviz是ROS可視化（ROSvisualization）的縮寫，是ROS中強(qiáng)有力的3D可視化工具。它使得用戶能夠查看模擬機(jī)器人模型、來自機(jī)器人傳感器的傳感器日志信息，并且重放已記錄的傳感器信息。用戶能夠?qū)C(jī)器人應(yīng)用程序進(jìn)行調(diào)試，調(diào)試內(nèi)容涵蓋從傳感器輸入到計劃動作(或計劃外動作)的整個過程。rvizrviz主界面劃分為以下幾個主要的顯示區(qū)域:1.中心窗口2.

Displays控制面板3.工具欄4.Views控制面板5.Time控制面板6.主窗口菜單欄。常見的display類型類型描述消息類型Axes顯示坐標(biāo)系－Gamera從相機(jī)視角顯示圖像sensor_msgs/Imagesensor_msgs/CameraInfoGrid顯示網(wǎng)格－Image顯示圖像sensor_msgs/ImageLaserScan顯示激光雷達(dá)數(shù)據(jù)sensor_msgs/LaserScanImage顯示圖像sensor_msgs/ImagePointClode2顯示點(diǎn)云數(shù)據(jù)sensor_msgs/PointCloud2Odomerty顯示里程計數(shù)據(jù)nav_msgs/OdometryPointClode2顯示點(diǎn)云數(shù)據(jù)sensor_msgs/PointCloud2RobotModel顯示機(jī)器人模型－PointClode2顯示TF樹－rviz的視角1.Orbital軌道攝像機(jī)視角，圍繞一個焦點(diǎn)旋轉(zhuǎn)，Ｌ:單擊并拖動圍繞焦點(diǎn)旋轉(zhuǎn)Ｍ:在相機(jī)向上和向右向量形成平面，按下中鍵并拖動焦點(diǎn)R:單擊拖動鼠標(biāo)放大/縮小焦點(diǎn)向上拖動放大，向下拖動縮小滾輪:放大縮小焦點(diǎn)2.FPS第一視角FPVＬ:單擊選中對象，并拖動旋轉(zhuǎn)Ｍ:移動沿著相機(jī)向上、向右向量形成的平面移動R:右擊并拖動，前后移動，上拖向前移動，下拖向后移動滾輪:前后移動3.top-downOrthographic自上而下的正視圖Ｌ:單擊并拖動繞Ｚ軸旋轉(zhuǎn)Ｍ:在ＸＹ平面移動R:右擊并拖動，縮放圖像滾輪:縮放圖像4.ＸＹOrthographic限制在XY平面GazeboGazebo是一款3D動態(tài)模擬器，能夠在復(fù)雜的室內(nèi)和室外環(huán)境中準(zhǔn)確有效地模擬機(jī)器人群。與游戲引擎提供高保真度的視覺模擬類似，Gazebo提供高保真度的物理模擬，其提供一整套傳感器模型，以及對用戶和程序非常友好的交互方式。1.Gazebo的典型用途●測試機(jī)器人算法●設(shè)計機(jī)器人●用現(xiàn)實(shí)場景進(jìn)行回歸測試2.Gazebo的一些主要特點(diǎn)●包含多個物理引擎●包含豐富的機(jī)器人模型和環(huán)境庫●包含各種各樣的傳感器●程序設(shè)計方便和具有簡單的圖形界面3.系統(tǒng)要求Gazebo官方建議，Gazebo目前最好在Ubuntu或者其他的Linux發(fā)行版上運(yùn)行。同時您的計算機(jī)需要具有以下功能：●專用GPU：Nvidia卡往往在Ubuntu中運(yùn)行良好●至少是IntelI5或同等產(chǎn)品的CPU●至少500MB的可用磁盤空間●安裝盡可能高版本的UbuntuTrusty02激光雷達(dá)傳感器激光雷達(dá)探測原理激光雷達(dá)（Lidar）類似于雷達(dá)（Radar），但是分辨率更高，因為激光的波長大約比無線電的波長小10萬倍?？梢杂脕韰^(qū)分真實(shí)移動中的行人和人物海報、在三維立體空間中建模、檢測靜態(tài)物體、精確測距。激光雷達(dá)是通過發(fā)射激光束來探測目標(biāo)位置、速度等特征量的雷達(dá)系統(tǒng)，具有測量精度高、方向性好等優(yōu)點(diǎn)。隼鳥2號LiDAR結(jié)合光學(xué)相機(jī)影像獲取龍宮全球三維地形信息iPadPro探測點(diǎn)云示意圖激光雷達(dá)優(yōu)點(diǎn)●具有極高的分辨率●抗干擾能力強(qiáng)●獲取的信息量豐富●可全天時工作激光雷達(dá)缺點(diǎn)●工作時受天氣和大氣影響大。激光一般在晴朗的天氣里衰減較小，傳播距離較遠(yuǎn)。而在大雨、濃煙、濃霧等壞天氣里，衰減急劇加大，傳播距離大受影響?！窦す饫走_(dá)的波束極窄,在空間搜索目標(biāo)非常困難，直接影響對非合作目標(biāo)的截獲概率和探測效率,只能在較小的范圍內(nèi)搜索、捕獲目標(biāo)，激光雷達(dá)RPLIDARA1高度60毫米直徑98.5毫米重量170克測量半徑范圍0.15-12米掃描測距角度360度測量頻率最大可達(dá)每秒8000次掃描頻率5.5赫茲。激光雷達(dá)RPLIDARA2高度41毫米直徑190毫米重量76克測量半徑范圍0.15-18米掃描測距角度360度測量頻率最大可達(dá)每秒8000次掃描頻率15赫茲。激光雷達(dá)RPLIDARA1/2是思嵐科技（SLAMTEC）有限公司的產(chǎn)品,能夠準(zhǔn)確測量試場中物體輪廓邊沿與設(shè)備間的相對距離，可快速獲得環(huán)境輪廓信息，這些輪廓信息組成所謂的點(diǎn)云并繪制出3D環(huán)境地圖，可以幫助機(jī)器人實(shí)現(xiàn)自主構(gòu)建地圖、實(shí)時路徑規(guī)劃與自動避開障礙物。03基于激光的Gmapping算法Gmapping是一種基于Rao-Blackwellized粒子濾波器并適用于網(wǎng)格地圖的SLAM算法，也是一種有效解決同時定位和建圖的算法，它將定位和建圖分離，并且每一個粒子都攜帶一幅地圖。

Gmapping算法流程圖重要性重采樣（SamplingImportanceResampling,SIR）濾波器

Gmapping算法的實(shí)現(xiàn)步驟1）第i個粒子在k時刻的預(yù)測位姿可以由k-1

時刻的位姿和控制輸入通過運(yùn)動模型得出。2）機(jī)器人基于地圖

，從預(yù)測的位姿處執(zhí)行掃描匹配算法。掃描匹配執(zhí)行的搜索區(qū)域被限定在附近的一個有限區(qū)域內(nèi)。如果掃描匹配失敗，則根據(jù)運(yùn)動模型計算位姿和權(quán)重（忽略步驟3和4）。3）通過掃描匹配器在位姿周圍選取一組采樣點(diǎn)，其中是掃描匹配器通過將當(dāng)前觀測值與已構(gòu)建的地圖進(jìn)行匹配，找到最有可能的位姿。基于這些點(diǎn)，通過逐點(diǎn)評估采樣位姿中的目標(biāo)分布來計算建議的平均值和協(xié)方差矩陣。在此階段計算加權(quán)系數(shù)。4）根據(jù)改進(jìn)的建議分布得出粒子i

的新位姿，位姿服從高斯分布的形式。5）更新粒子權(quán)重。6）根據(jù)機(jī)器人位姿和最新觀測，更新粒子i的地圖。7）得到新粒子樣本后，根據(jù)閾值執(zhí)行重采樣步驟。04基于激光的HectorSLAM算法HectorSLAM使用一種快速在線學(xué)習(xí)的占用柵格地圖系統(tǒng)，它是結(jié)合了激光雷達(dá)系統(tǒng)和基于慣性傳感的三維姿態(tài)估計系統(tǒng)的魯棒掃描匹配方法。通過使用快速近似的地圖梯度和多分辨率柵格，實(shí)現(xiàn)了可靠的定位和建圖。HectorSLAM功能包使用高斯牛頓的方法，不需要里程計數(shù)據(jù)，只根據(jù)激光信息便可構(gòu)建地圖。

SLAM一般分為前端和后端系統(tǒng)。SLAM前端用于實(shí)時在線估計機(jī)器人運(yùn)動，后端用于對位姿圖進(jìn)行優(yōu)化。前端快速掃描匹配步驟用于位姿估計，而較慢的后端建圖步驟在后臺或遠(yuǎn)程計算機(jī)上運(yùn)行。HectorSLAM方法主要關(guān)注前端系統(tǒng)，沒有提供相應(yīng)的位姿圖優(yōu)化，主要是由于這種優(yōu)化在真實(shí)環(huán)境下建圖是不需要的，因為HectorSLAM方法對于機(jī)器人執(zhí)行任務(wù)來說已經(jīng)足夠精確。HectorSLAM算法原理HectorSLAM系統(tǒng)需要用到6自由度運(yùn)動的平臺，這不同于其它2D網(wǎng)格SLAM算法所假設(shè)的3自由度運(yùn)動，因此系統(tǒng)必須估計由平臺的平移和旋轉(zhuǎn)組成的全6自由度狀態(tài)。為此，該系統(tǒng)由兩個主要組件組成，導(dǎo)航濾波器子系統(tǒng)融合來自慣性測量單元和其他可用傳感器的信息，形成一致的3D解決方案，而2DSLAM子系統(tǒng)用于提供地平面內(nèi)的位姿和航向信息。這兩種估計都是單獨(dú)更新的，并且只是松散耦合的，因此它們隨著時間的推移保持同步。Hectorslam算法的實(shí)現(xiàn)步驟（1）在2DSLAM中，為了能夠表示任意環(huán)境，HectorSLAM使用占用柵格地圖，占用柵格地圖的離散特性限制了地圖表達(dá)精度，也不允許直接計算插值或?qū)?shù)。給定一個連續(xù)的地圖坐標(biāo)，占用值以及梯度可以如圖用四個近鄰點(diǎn)來近似，沿著x軸和y軸進(jìn)行線性插值，然后得到:導(dǎo)數(shù)可以近似為（2）掃描匹配是將激光掃描相互對齊或與現(xiàn)有地圖對齊的過程，HectorSLAM實(shí)質(zhì)上是一個將掃描末端點(diǎn)與已知地圖對準(zhǔn)優(yōu)化方法,其基本思路是使用高斯-牛頓法。通過解最小二乘函數(shù)，使得激光掃描與地圖有最佳的對齊：代表機(jī)器人的位姿，代表i號激光束在機(jī)器人姿態(tài)下掃描點(diǎn)的世界坐標(biāo)，其坐標(biāo)變化為：根據(jù)優(yōu)化測量誤差來實(shí)現(xiàn)對的估計：對做一階泰勒展開得：對上式求偏導(dǎo)并設(shè)置為，可得上述最小化問題的高斯牛頓方程：其中（3）HectorSLAM使用擴(kuò)展卡爾曼濾波器(EKF)。在掃描時的卡爾曼估計可以用均值和協(xié)方差來表示，位姿估計

直接得出，融合結(jié)果如下：觀測器矩陣C將整個狀態(tài)空間投影到SLAM系統(tǒng)的三維子空間中，參數(shù)用來調(diào)整SLAM更新的效果。于是，進(jìn)一步有：05基于激光的Cartographer算法基于激光的SLAM方法中，掃描--掃描匹配（Scan-to-scanmatching）經(jīng)常用于計算相對姿態(tài)變化。然而，掃描--掃描匹配本身會很快積累誤差。掃描--地圖匹配（Scan-to-mapmatching）有助于限制這種誤差積累，可使用高斯-牛頓方法在線性插值圖上尋找局部最優(yōu)解。當(dāng)高精度頻率激光雷達(dá)提供良好的位姿初始估計時，局部優(yōu)化的掃描--地圖匹配是有效和魯棒的。

像素精度的掃描匹配方法，可以進(jìn)一步減少了局部誤差積累。解決剩余局部誤差累積的兩種常見方法是粒子濾波和基于圖形的SLAM。Cartographer是谷歌推出的一套基于圖優(yōu)化的SLAM算法。Cartographer的設(shè)計目的是在計算資源有限的情況下，實(shí)時獲取相對較高精度的2D地圖?？紤]到基于模擬策略的粒子濾波方法在較大環(huán)境下對內(nèi)存和計算資源的需求較高，Cartographer采用基于圖優(yōu)化方法。Cartographer是一個實(shí)時的室內(nèi)建圖算法，能生成分辨率的柵格地圖。在前端將最新的激光雷達(dá)掃描數(shù)據(jù)在相鄰的子圖上(整個地圖的一小塊)完成掃描匹配，得到一個在短時間內(nèi)準(zhǔn)確的最佳插入位置(位姿)后，將掃描插入到子圖中。掃描匹配中，位姿估計的誤差會在整個地圖中隨時間逐漸累積，在后端中,通過回環(huán)檢測加約束進(jìn)行優(yōu)化消除誤差。Cartographer算法框架圖Cartographer算法原理Cartographer系統(tǒng)將獨(dú)立的局部和全局方法綜合來實(shí)現(xiàn)二維的定位與地圖構(gòu)建。在局部方法中，每個連續(xù)的掃描都與世界坐標(biāo)系上的一小塊區(qū)域（稱為子圖）進(jìn)行匹配，使用非線性優(yōu)化將掃描與子圖對齊，此過程被稱為掃描匹配。子圖的構(gòu)建就是將掃描結(jié)果不斷與子圖的坐標(biāo)系對齊的迭代過程。將掃描原點(diǎn)設(shè)為，雷達(dá)掃描點(diǎn)記為，。變換表示的是位姿位于掃描幀中的位置轉(zhuǎn)換到子圖幀中的位置，于是，對于一個雷達(dá)掃描點(diǎn)，轉(zhuǎn)換到子圖幀中可以用下式表示：連續(xù)幀掃描可以用來構(gòu)建一個子圖，這些子圖采用概率網(wǎng)格

:的形式，r是網(wǎng)格地圖的分辨率網(wǎng)格點(diǎn)和相關(guān)像素

網(wǎng)格點(diǎn)所對應(yīng)計算的值代表存在障礙物的概率。對于每個網(wǎng)格點(diǎn)定義相應(yīng)的像素，它是由離網(wǎng)格點(diǎn)最近的所有點(diǎn)組成的。每一個像素是尺寸為的方格，它代表對應(yīng)范圍內(nèi)的所有點(diǎn)，掃描數(shù)據(jù)點(diǎn)是實(shí)數(shù)，概率網(wǎng)格的尺寸是整數(shù)。每當(dāng)掃描要插入到概率網(wǎng)格地圖時，都要確定命中（hits）的點(diǎn)和未命中（misses）的點(diǎn)集。命中就是網(wǎng)格里有雷達(dá)數(shù)據(jù)點(diǎn)，未命中就是沒雷達(dá)數(shù)據(jù)點(diǎn)。每一個Hit的點(diǎn)（陰影且畫的），對應(yīng)的最近鄰格子點(diǎn)加入到命中點(diǎn)集合；每一個Miss的點(diǎn)（陰影），雷達(dá)原點(diǎn)到掃描點(diǎn)中間的格子點(diǎn)全部加到未命中點(diǎn)集合，如果這個點(diǎn)已經(jīng)在命中點(diǎn)集中則排除。與hit和miss相關(guān)的掃描和像素每個還未觀察到的網(wǎng)格，可以自定義賦值為一個或者值（一般用0.5，即不知道這個網(wǎng)格有沒有被占用）。已經(jīng)觀察到的網(wǎng)格，它的概率更新為：每個網(wǎng)格的命中概率更新公式為：一幀掃描插入子圖之前，首先要對這幀掃描的位姿進(jìn)行優(yōu)化。Cartographer使用的是基于Ceres庫的掃描匹配。該優(yōu)化問題可以描述成一個非線性最小二乘問題：與掃描匹配一樣，環(huán)路閉合優(yōu)化也可描述為一個非線性最小二乘問題，每間隔幾秒，就用Ceres庫來計算出一個解：殘差的計算：當(dāng)掃描匹配將不正確的約束添加到優(yōu)化問題時，損失函數(shù)（例如Huberloss）可以用于減少異常值的影響。使用基于分支定界（Branch-and-bound）掃描匹配方法，得到精確到像素級的最優(yōu)位姿估計，核心公式如下：分支定界算法的基本步驟分支定界法是求解整數(shù)規(guī)劃問題的最常用算法，這種方法不但可以求解純整數(shù)規(guī)劃，還可以求解混合整數(shù)規(guī)劃問題。分支定界法采用搜索與迭代的方法，選擇不同的分支變量和子問題進(jìn)行分支?；谝曈X的SLAM技術(shù)第五章目錄CONTENTS01經(jīng)典視覺SLAM框架02MonoSLAM算法03ORB-SLAM204多機(jī)器人視覺SLAM技術(shù)01經(jīng)典視覺SLAM框架視覺SLAM框架前端：VO后端：Optimization回環(huán)檢測Loop

Closing建圖Mapping視覺傳感器1.單目相機(jī)模型某一相機(jī)拍攝時，若外部所有可視景物光線只能通過相機(jī)的光心到達(dá)相機(jī)的成像平面并形成倒立縮小的成像平面點(diǎn)，這種相機(jī)就屬于針孔模型相機(jī)幾何關(guān)系:2.雙目相機(jī)模型雙目相機(jī)一般是由左右兩個單目相機(jī)組成，兩個相機(jī)水平放置，當(dāng)然也還有其他放置方式，比如上下兩目，本質(zhì)上沒有什么區(qū)別。雙目相機(jī)雙目相機(jī)成像原理幾何關(guān)系:3.深度相機(jī)模型深度相機(jī)能夠主動測量每個像素的深度信息紅外結(jié)構(gòu)光來測量像素距離（Kinect1代相機(jī)）通過飛行時間法原理來測量像素距離（Kinect2代相機(jī)）●在基于紅外結(jié)構(gòu)光的原理中，相機(jī)主要是根據(jù)返回的結(jié)構(gòu)光的圖案來計算物體與相機(jī)之間的距離?！窕陲w行時間法原理的相機(jī)主要是通過向目標(biāo)發(fā)射脈沖光，然后根據(jù)發(fā)送到返回之間光束的飛行時間來確定物體和相機(jī)之間的距離的。視覺里程計VisualOdometry相鄰圖像估計相機(jī)運(yùn)動基本形式：通過兩張圖像計算運(yùn)動和結(jié)構(gòu)不可避免地有漂移方法●特征點(diǎn)法

●直接法后端優(yōu)化?濾波?BA圖優(yōu)化（視覺主流方法）1.BA代價函數(shù)與圖優(yōu)化（1）1.投影模型第一步，把世界坐標(biāo)轉(zhuǎn)換到相機(jī)坐標(biāo)系：第二步，將投至歸一化平面，得到歸一化坐標(biāo)：第三步，考慮歸一化坐標(biāo)的徑向畸變情況，可以用一個多項式函數(shù)來描述畸變前后的坐標(biāo)變化：第四步，根據(jù)內(nèi)參模型，計算像素坐標(biāo)：（2）BA優(yōu)化函數(shù)投影的整個過程看似有些復(fù)雜，可以將整個過程概括為一個觀測方程，可以把它抽象成：關(guān)于觀測的誤差：整體的代價函數(shù)可以定義為：2.BA求解把自變量定義成所有待優(yōu)化的變量：給自變量一個增量時，目標(biāo)函數(shù)變?yōu)椋喊芽臻g點(diǎn)的變量也放在一起，把相機(jī)位姿變量放到一起：公式可以簡化表達(dá)：3.位姿圖優(yōu)化相機(jī)的位姿以及位姿節(jié)點(diǎn)之間相對運(yùn)動的估計:按照李群的寫法來表示:構(gòu)建誤差:將擾動項移至兩側(cè)，根據(jù)伴隨性質(zhì)，可以得到：稍加整理可以得到：求出誤差關(guān)于兩個位姿的雅可比矩陣：取近似：總體目標(biāo)函數(shù)為：將擾動項移至最后，導(dǎo)出右乘形式的雅可比矩陣：回環(huán)檢測檢測機(jī)器人是否回到早先位置識別到達(dá)過的場景計算圖像間的相似性方法：詞袋模型建圖用于導(dǎo)航、規(guī)劃、通訊、可視化、交互等度量地圖vs拓?fù)涞貓D稀疏地圖vs稠密地圖02MonoSLAM算法MonoSLAM是由AndrewJ.Davison等人在2007年提出的一種在未知場景中恢復(fù)單目相機(jī)3D移動軌跡的實(shí)時算法。該方法的核心是在概率框架內(nèi)在線創(chuàng)建一個稀疏持久的地圖路標(biāo)。?采用一種主動的地圖構(gòu)建與測量方法?使用一般的平滑相機(jī)運(yùn)動模型來捕捉視頻流中固有的動力學(xué)先驗信息?徹底解決單目特征初始化問題。MonoSLAM系統(tǒng)圖MonoSLAM算法1、概率三維地圖MonoSLAM是基于概率特征的建圖，地圖中的信息代表相機(jī)狀態(tài)和所有感興趣的特征的當(dāng)前估計，還代表這些估計中的不確定性。地圖可以狀態(tài)向量和協(xié)方差矩陣表示：相機(jī)的狀態(tài)向量：2、自然視覺路標(biāo)●MonoSLAM的目標(biāo)是在相機(jī)可能劇烈運(yùn)動下能夠重復(fù)識別一樣的視覺地標(biāo)●假定每一個特征都在一個局部平面上●在初始化階段，認(rèn)為局部平面的法向與特征至相機(jī)矢量平行3、系統(tǒng)初始化在單目相機(jī)SLAM算法中，初始化時通常在相機(jī)前面放置一個已知物體作為先驗信息●在單目相機(jī)SLAM中，沒有直接的方法來測量特征深度或里程信息●初始化時，已知特征信息可有助于直接進(jìn)入預(yù)測、測量、更新的模式4、運(yùn)動模型和預(yù)測MonoSLAM采用恒定速度、恒定角速度模型噪聲為：狀態(tài)更新方程為：通過雅可比矩陣計算得到過程噪聲協(xié)方差：5、主動特征測量與地圖更新相機(jī)的位置：圖像中特征的位置：徑向畸變模型：特征在圖像上位置預(yù)測的不確定性用對稱信息協(xié)方差矩陣表示：6、特征初始化MonoSLAM中，在新特征首個測量和確認(rèn)后，在地圖中初始化一條3D線并使得該特征在此線上。這是一條半無限線，起點(diǎn)為相機(jī)的估計位置，沿著特征點(diǎn)視覺方向并指向無窮遠(yuǎn)處7、地圖管理●對地圖中特征數(shù)量的管理，需要動態(tài)地決定何時應(yīng)該識別和初始化新特征，何時需要刪除某個特征?！袷箯娜魏蜗鄼C(jī)位置所可見的可靠特征的數(shù)量接近預(yù)定值●只有當(dāng)相機(jī)經(jīng)過的區(qū)域中可見的特征數(shù)量小于閾值時，才會將特征添加到地圖中8、特征方位估計?當(dāng)相機(jī)移動時，它的外觀會隨著視角的變化而改變。?扭曲的量化取決于相機(jī)的初始和當(dāng)前位置，特征中心的三維位置以及其局部表面的方位。?SLAM系統(tǒng)提供了實(shí)時的相機(jī)位姿和3D特征位置的估計，為每一個特征點(diǎn)都保留了相機(jī)初始位置和局部平面方位的估計。在兩個不同位置觀測定向平面幾何態(tài)勢，于是，扭曲可由單應(yīng)性來描述：平面特征表面三維方位估計流程假設(shè)外觀預(yù)測能保證特征的當(dāng)前圖像位置能定位，下一步則是測量預(yù)測模板和當(dāng)前圖像之間的扭曲變化。通過假設(shè)扭曲變化小，并采用概率逆合成梯度下降圖像對齊步驟進(jìn)行搜索來達(dá)到全局最佳擬合。03ORBSLAM2算法ORB-SLAM2是一款基于單目、雙目、RGB-D相機(jī)的SLAM系統(tǒng)ORB-SLAM2是在單目ORB-SLAM的基礎(chǔ)上提出的?回環(huán)檢測?地圖重用?重定位算法流程圖ORB-SLAM主線線程1、跟蹤線程跟蹤線程主要負(fù)責(zé)對每一幀圖像的相機(jī)位姿進(jìn)行定位，同時決策何時插入新的關(guān)鍵幀。（1）特征提?。?）局部地圖初始化（3）相機(jī)位姿計算（4）局部地圖跟蹤（5）是否插入關(guān)鍵幀判斷插入關(guān)鍵幀的條件:1）局部建圖線程閑置或距離上次插入關(guān)鍵幀時間超過了20幀；2）距離最近一次相機(jī)重定位處理超過了20幀；3）當(dāng)前幀中至少跟蹤50個ORB特征關(guān)鍵點(diǎn)；4）當(dāng)前幀中跟蹤的特征點(diǎn)的數(shù)量至少有10%不同于參考關(guān)鍵幀。2、局部建圖線程（1）關(guān)鍵幀插入（2）地圖點(diǎn)云篩選（3）新地圖點(diǎn)云創(chuàng)建（4）局部BA（5）局部關(guān)鍵幀篩選3、閉環(huán)檢測（1）候選關(guān)鍵幀（2）計算相似變換（3）回環(huán)融合（4）本征圖優(yōu)化ORB-SLAM2主要模塊4、單目、雙目近處和雙目遠(yuǎn)處特征點(diǎn)?ORB-SLAM2直接對輸入圖像進(jìn)行預(yù)處理，提取一些關(guān)鍵位置上的特征，輸入圖像直接被棄用?ORB-SLAM2能處理單目或者雙目特征點(diǎn)，進(jìn)一步分成遠(yuǎn)處特征點(diǎn)和近處特征點(diǎn)兩類。5、單目與雙目約束的光束平差法?采用光束平差法（BA）來優(yōu)化跟蹤線程中相機(jī)的位姿（純運(yùn)動BA）?優(yōu)化關(guān)鍵幀的本地窗口和局部建圖線程的特征點(diǎn)（局部BA）?在回環(huán)檢測之后優(yōu)化所有的關(guān)鍵幀和特征點(diǎn)（全局BA）1.純運(yùn)動BA最小化世界坐標(biāo)系下3D點(diǎn)與特征點(diǎn)之間的重投影誤差來優(yōu)化相機(jī)旋轉(zhuǎn)矩陣和平移矩陣投影函數(shù)單目用表示，雙目用表示2.局部BA優(yōu)化的是一組共視關(guān)鍵幀以及在這些關(guān)鍵幀所觀測到的特征點(diǎn)3.全局BA是局部BA的一個特例，除了初始幀，所有的關(guān)鍵幀和地圖中所有點(diǎn)都會被優(yōu)化6、回環(huán)檢測和全局BA閉環(huán)分兩步執(zhí)行?閉環(huán)必須被檢測和確認(rèn)?通過優(yōu)化一個位姿圖去矯正閉環(huán)在ORB-SLAM2的位姿優(yōu)化后，系統(tǒng)采用全局的BA優(yōu)化以獲取最優(yōu)解。

7、關(guān)鍵幀插入若跟蹤的近點(diǎn)數(shù)目小于某一個閾值，且這個幀能產(chǎn)生至少個新近雙目特征點(diǎn)，此時將會插入一個新的關(guān)鍵幀。經(jīng)驗值認(rèn)為，當(dāng)和時效果最好。8、定位模式只要環(huán)境沒有明顯變化，該模式就可用于在已知地圖區(qū)域進(jìn)行輕量級的長期定位。在該模式中，局部建圖和回環(huán)檢測線程停用，相機(jī)始終可以通過跟蹤進(jìn)行重定位。04多機(jī)器人視覺SLAM技術(shù)1.多機(jī)器人系統(tǒng)將多機(jī)器人系統(tǒng)與SLAM技術(shù)相結(jié)合，機(jī)器人之間相互交換信息，共享觀測信息來協(xié)助對方相互定位并構(gòu)建環(huán)境地圖。集中式分布式混合式2.機(jī)器人相互識別（1）目標(biāo)檢測方案?當(dāng)機(jī)器人成員相互進(jìn)入視野時，能夠從圖像中把機(jī)器人從環(huán)境背景中分離出來，從而達(dá)到識別的目的?基于深度學(xué)習(xí)的目標(biāo)檢測，相比于其它方案來說，檢測效果更好，檢測的準(zhǔn)確率得到了大幅提升?以YOLO（YouOnlyLookOnce）為代表的深度學(xué)習(xí)檢測方案（2）YOLO基本原理?首先調(diào)整圖像大小?然后將圖像輸入卷積神經(jīng)網(wǎng)絡(luò)?最好根