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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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