基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建

分類號(hào)：?jiǎn)挝淮a： 申 請(qǐng) 號(hào)： 本科學(xué)位論文 中文論文題目中文論文題目:? 英文論文題目英文論文題目:Indoor space mapping based on aircraft platform 申請(qǐng)人姓名：? 指導(dǎo)教師：? ：專業(yè)名稱：? 研究方向：? 所在學(xué)院：? 論文提交日期論文提交日期 ? 近年來(lái)，飛行器自主導(dǎo)航中的同步定位與地圖構(gòu)建（SLAM，Simultaneous Localization and Mapping）成為了研究的熱點(diǎn)。由于其重要的理論與應(yīng)用價(jià)值, 被很多學(xué)者認(rèn)為是實(shí)現(xiàn) 全自主移動(dòng)機(jī)器人的關(guān)鍵。SLAM 算法目前正逐步向飛行器領(lǐng)域拓展，對(duì)引導(dǎo)無(wú)人機(jī)自主 探索未知環(huán)境具有極其重要的意義。 本文首先對(duì)機(jī)器人 SLAM 問(wèn)題進(jìn)行了概述，給出了 SLAM 問(wèn)題的概率模型，介紹了 基于濾波的方法和基于圖優(yōu)化的方法。然后本文對(duì)圖優(yōu)化的方法進(jìn)行了詳細(xì)的描述。本文 介紹了基于圖優(yōu)化的 SLAM 框架，對(duì)圖優(yōu)化問(wèn)題進(jìn)行建模，使用了一種基于最小二乘的優(yōu) 化算法解決圖優(yōu)化 SLAM 問(wèn)題。然后本文使用公開(kāi)的數(shù)據(jù)集對(duì)這一優(yōu)化算法進(jìn)行測(cè)試。 在算法測(cè)試完成之后，本文將該算法做了實(shí)際的應(yīng)用。本文前端借助單目視覺(jué)方法， 后端使用基于圖的優(yōu)化方法，對(duì)室內(nèi)環(huán)境進(jìn)行地圖構(gòu)建。首先使用手持?jǐn)z像頭對(duì)桌面環(huán)境 進(jìn)行了實(shí)時(shí)地圖構(gòu)建。然后搭建飛行器平臺(tái)，使用飛行器自帶的前置攝像頭作為傳感器， 通過(guò) wifi 將圖像信息會(huì)傳到地面站，地面站作為處理單元，對(duì)飛行器所處環(huán)境進(jìn)行地圖構(gòu) 建。 關(guān)鍵詞： SLAM，圖優(yōu)化，飛行器地圖構(gòu)建 i Abstract Recent years, Simultaneous Localization and Mapping (SLAM) has become a research hot spot. Due to its important application value, SLAM is considered as a major point of autonomous mobile robot. SLAM algorithm is gradually developing in the field of aircraft, which has signifi- cance to guide in UAV autonomously exploring in unknown environment. First, this paper gives a brief review of SLAM, forms up a probability model, introduces filter- based method and graph-based method. Second, this paper describes the graph-based SLAM in detail. We introduce the framework of the graph-based SLAM, model the optimization problem, and use the least squares algorithm to solve this problem. Third this paper uses public data sets to test this optimization algorithm. After the algorithm test is completed, this paper applies this algorithm in real environment. We map the indoor environment using monocular vision method to process data, and graph-based method to optimize the map. At first, we hold the camera in hand, and build the real-time map of the desktop environment. Then we construct an aircraft platform, using aircrafts front-facing camera as main sensor to transmits image data to the ground station which builds the real-time map of the environment. Keywords:SLAM, Graph-based optimization, Aircraft map-building iii 目?目? ? .i Abstractiii 目? 1?論 .1 1.1?題?1 1.2SLAM ?2 1.3?文?3 2SLAM ?題? .5 2.1SLAM ?題?5 2.2SLAM ?題?6 3? SLAM ? 9 3.1? SLAM ?.9 3.2? SLAM ?. 10 3.3? 12 3.4?題? 13 3.5? 14 3.6? 19 4? SLAM ? 23 4.1? 23 4.2? 24 4.3? 29 4.4? 29 5? . 37 5.1? 37 5.2? 37 ?文? . 39 v 1?論?論 1.1?題?題? 由于體積小、機(jī)動(dòng)性能好，空中機(jī)器人成為了大量室內(nèi)和室外任務(wù)的理想平臺(tái)。尤其 在不適宜地面機(jī)器人行走的復(fù)雜環(huán)境中，空中機(jī)器人的優(yōu)勢(shì)更加明顯。在這些任務(wù)中，空 中機(jī)器人自主飛行的能力至關(guān)重要。由于 GPS 技術(shù)，我們看到了很多成功的空中飛行器的 戶外應(yīng)用，如航拍、運(yùn)輸、偵查、搜救等。但在缺少 GPS 信號(hào)的室內(nèi)環(huán)境中，僅依靠自身 慣性測(cè)量系統(tǒng)，飛行器無(wú)法自身定位更無(wú)法估計(jì)周圍環(huán)境。因此，為實(shí)現(xiàn)空中機(jī)器人在缺 少 GPS 信號(hào)的未知環(huán)境中的自主導(dǎo)航，我們需要飛行器對(duì)自身狀態(tài)信息和環(huán)境的感知，建 立周圍環(huán)境的模型。 在未知環(huán)境中，移動(dòng)機(jī)器人為了能夠探索未知區(qū)域并順利完成給定的任務(wù)，需要具有 自主導(dǎo)航能力。圖 1.1 描述了基于地圖的移動(dòng)機(jī)器人導(dǎo)航結(jié)構(gòu)，其中定位（Localization）是 指機(jī)器人確定自身在工作環(huán)境中的精確位姿；地圖構(gòu)建（Mapping）是指建立機(jī)器人所工 作環(huán)境的各種物體，如障礙、路標(biāo)等的準(zhǔn)確的空間位置描述，即環(huán)境地圖。如圖 1.1 所示， 移動(dòng)機(jī)器人精確的定位以及環(huán)境地圖創(chuàng)建是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的前提和基礎(chǔ)。 圖 1.1基于地圖的機(jī)器人導(dǎo)航結(jié)構(gòu) 移動(dòng)機(jī)器人的精確定位是基于對(duì)環(huán)境的感知和理解，需要?jiǎng)?chuàng)建環(huán)境地圖，而環(huán)境地 圖的創(chuàng)建又需要精確的定位。因此，移動(dòng)機(jī)器人的定位和地圖創(chuàng)建是一個(gè)既矛盾又相 1 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 關(guān)的過(guò)程。為了克服這個(gè)問(wèn)題，一些研究者提出了移動(dòng)機(jī)器人的同步定位與地圖構(gòu)建1 （SLAM，Simultaneous Localization and Mapping） 。由于其重要的理論與應(yīng)用價(jià)值, 被很多學(xué) 者認(rèn)為是實(shí)現(xiàn)真正全自主移動(dòng)機(jī)器人的關(guān)鍵。SLAM 問(wèn)題，最早是由 Randall Smith 和 Peter Cheseseman 在 1988 年發(fā)表的論文1當(dāng)中提出來(lái)。在他們的論文中，利用移動(dòng)機(jī)器人的運(yùn) 動(dòng)方式和裝備的傳感器獲得的測(cè)量數(shù)據(jù)，分別設(shè)計(jì)了移動(dòng)機(jī)器人的運(yùn)動(dòng)模型和觀測(cè)模型， 結(jié)合概率學(xué)的貝葉斯理論，實(shí)現(xiàn)了對(duì)輪式移動(dòng)機(jī)器人在未知環(huán)境中的運(yùn)動(dòng)狀態(tài)進(jìn)行實(shí)時(shí)估 計(jì)。所有移動(dòng)機(jī)器人導(dǎo)航的基本過(guò)程可以總結(jié)為三個(gè)最基本的關(guān)鍵問(wèn)題，即“Where am I now?” 、 “What is the structure of my environment? ”以及“How can I get that target position?” 這三個(gè)問(wèn)題的實(shí)質(zhì)指的就是機(jī)器人定位和地圖創(chuàng)建， 以及機(jī)器人導(dǎo)航路徑規(guī)劃問(wèn)題。 SLAM 問(wèn)題的解決直接影響后面路徑規(guī)劃的問(wèn)題能否正確處理，所以說(shuō)，移動(dòng)機(jī)器人的同步定位 與地圖構(gòu)建是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航，提高機(jī)器人的智能化水平的關(guān)鍵和首要解決的基礎(chǔ)性 難題。 SLAM 算法目前正逐步向飛行器領(lǐng)域拓展，對(duì)引導(dǎo)無(wú)人機(jī)自主探索未知環(huán)境具有極其 重要的意義。 1.2SLAM ? 移動(dòng)機(jī)器人 SLAM 問(wèn)題的研究始于上世紀(jì)八十年代中期。在 1986 年的 ICRA（Interna- tional Conference of Robotics and Automation）會(huì)議上，Peter Cheeseman, Jim Crowley, Hugh Durrant-Whyte2提出了利用貝葉斯估計(jì)理論完成移動(dòng)機(jī)器人狀態(tài)和地圖創(chuàng)建同時(shí)估計(jì)方 法的可能性。 1988 年，Smith,Self 和 Cheeseman1將 SLAM 問(wèn)題系統(tǒng)地表示為隨機(jī)估計(jì)問(wèn)題，用一 個(gè)通用的狀態(tài)向量以及該向量的協(xié)方差矩陣表示環(huán)境路標(biāo)位置和機(jī)器人位置。該論文奠定 了移動(dòng)機(jī)器人 SLAM 問(wèn)題以后研究實(shí)現(xiàn)的總體方法。1995 年，Durrant-Whyte3首次提出 解決 SLAM 問(wèn)題的一種系統(tǒng)框架，并證明 SLAM 最終會(huì)收斂。文章同時(shí)指出：一旦將機(jī) 器人狀態(tài)與路標(biāo)位置放入全狀態(tài)估計(jì)器中，最終狀態(tài)是收斂的，路標(biāo)間多次觀測(cè)的關(guān)聯(lián)性 越大，SLAM 將獲得更好的收斂性能。然而，在 SLAM 問(wèn)題中，由于全狀態(tài)估計(jì)使得被估 狀態(tài)維數(shù)隨著路標(biāo)數(shù)成平方增長(zhǎng)，因而必須面對(duì)計(jì)算效率低的問(wèn)題，這也就極大地限制了 SLAM 技術(shù)的發(fā)展和應(yīng)用。 本世紀(jì)初，研究者對(duì) SLAM 問(wèn)題的興趣空前高漲，主要致力于如何有效的減少計(jì)算 量，研究更高效的算法以及擴(kuò)大 SLAM 技術(shù)的應(yīng)用環(huán)境。目前降低 SLAM 計(jì)算復(fù)雜度的方 法主要集中于減少次要特征、改進(jìn)地圖表示方法以及更高效的算法等方面4。在當(dāng)前研究 2 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 領(lǐng)域，一般將 SLAM 問(wèn)題劃分為在線 SLAM 問(wèn)題（on-line SLAM problem）和全局 SLAM 問(wèn)題（full SLAM problem） 。對(duì)于在線 SLAM 問(wèn)題，待估計(jì)狀態(tài)量由機(jī)器人當(dāng)前位姿以及 構(gòu)成地圖的各路標(biāo)位置兩部分組成。解決這類問(wèn)題比較典型的做法就是采用濾波技術(shù)對(duì)狀 態(tài)量進(jìn)行迭代估計(jì)，每當(dāng)機(jī)器人獲取到新的觀測(cè)信息時(shí)，在線 SLAM 算法將對(duì)狀態(tài)量進(jìn)行 更新。在線 SLAM 問(wèn)題的相關(guān)研究中，比較有代表性的方法包括基于卡爾曼濾波器的方 法5，基于信息濾波器的方法6以及基于粒子濾波器的方法7。這些方法均以增量式的形 式進(jìn)行計(jì)算與更新，因此具備在線計(jì)算的特點(diǎn)。 與在線 SLAM 問(wèn)題相對(duì)應(yīng)，全局 SLAM 問(wèn)題要求對(duì)機(jī)器人整個(gè)導(dǎo)航任務(wù)期間各個(gè)時(shí) 刻的位姿以及各路標(biāo)的位置進(jìn)行估計(jì)，這類問(wèn)題在計(jì)算過(guò)程中主要包括數(shù)據(jù)關(guān)聯(lián)（Data Association）和優(yōu)化（Optimization）兩個(gè)過(guò)程。其中數(shù)據(jù)關(guān)聯(lián)過(guò)程負(fù)責(zé)將機(jī)器人狀態(tài)，路 標(biāo)以及里程計(jì)信息整理成因子圖的形式。因子圖一般包含兩類節(jié)點(diǎn)，它們分別表示機(jī)器人 各個(gè)時(shí)刻的位姿狀態(tài)或者路標(biāo)的位置狀態(tài)；因子圖中的邊則表示由其相連節(jié)點(diǎn)對(duì)應(yīng)時(shí)刻傳 感器測(cè)量信息匹配生成的空間約束條件。由于傳感器的測(cè)量過(guò)程中包含噪聲，因子圖中對(duì) 應(yīng)的空間約束往往同節(jié)點(diǎn)之間的相對(duì)空間關(guān)系不一致的。而 SLAM 中優(yōu)化中優(yōu)化過(guò)程則正 是針對(duì)這種“不一致” ，借助優(yōu)化的方法，重新配置圖中節(jié)點(diǎn)位置，使上述“不一致”最小 化。由于全局 SLAM 問(wèn)題通?？衫靡蜃訄D模型進(jìn)行描述，因此一般也將該類 SLAM 問(wèn) 題成為基于圖的 SLAM 問(wèn)題。在基于圖的 SLAM 問(wèn)題中，最具代表性的為 g2o（General Framework for Graph Optimiazion） 8。g2o 是一個(gè)用于處理圖優(yōu)化問(wèn)題的開(kāi)源框架，它通過(guò) 非線性最小二乘優(yōu)化和稀疏線性代數(shù)，實(shí)現(xiàn)了對(duì)基于圖的 SLAM 問(wèn)題的高效求解。 在過(guò)去，由于基于圖的方法需要解決多元變量非線性最小二乘優(yōu)化問(wèn)題，所以被認(rèn)為 復(fù)雜度高，計(jì)算量大，太耗費(fèi)時(shí)間，無(wú)法滿足 SLAM 的實(shí)時(shí)性要求。但隨著計(jì)算機(jī)計(jì)算能 力的增強(qiáng)以及高效求解方法的出現(xiàn)，基于圖優(yōu)化的方法在近幾年重新得到重視。目前，該 算法的應(yīng)用較少。本文嘗試使用 11 年德國(guó)弗萊堡大學(xué)自主智能實(shí)驗(yàn)室提出了 g2o 方法，做 一次飛行器平臺(tái)的實(shí)際應(yīng)用。 1.3?文?文? 本論文圍繞室內(nèi)環(huán)境下飛行器的 SLAM 問(wèn)題展開(kāi)，選擇合適的飛行器，利用單目攝像 機(jī)作為移動(dòng)機(jī)器人的外傳感器，提取室內(nèi)深度信息及視覺(jué)里程計(jì)數(shù)據(jù)，應(yīng)用基于圖優(yōu)化的 算法，搭建一套完整的能夠完成室內(nèi)地圖構(gòu)建的飛行器平臺(tái)。 具體內(nèi)容安排如下： 第一章首先介紹本論文的研究背景及意義，接著介紹移動(dòng)機(jī)器人 SLAM 問(wèn)題的研究進(jìn) 3 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 展，最后給出本論文的研究?jī)?nèi)容與結(jié)構(gòu)。 第二章對(duì) SLAM 問(wèn)題進(jìn)行了分析，介紹了 SLAM 問(wèn)題的一般模型和基本原理。 第三章分析了基于圖的 SLAM 算法，詳細(xì)介紹了該算法的思想與實(shí)現(xiàn)方法，并進(jìn)行仿 真實(shí)驗(yàn)。 第四章搭建飛行器平臺(tái)，并利用單目攝像機(jī)作為傳感器，使用基于圖的 SLAM 算法， 構(gòu)建室內(nèi)環(huán)境地圖。 第五章為總結(jié)與展望。首先對(duì)本文所作的工作進(jìn)行總結(jié)，然后對(duì)后繼工作的研究?jī)?nèi)容 進(jìn)行展望。 4 2SLAM ?題?題? 2.1SLAM ?題?題? 同步定位和地圖構(gòu)建（Simultaneous Localization and Mapping, SLAM）, 其基本思想是 自主機(jī)器人在全然未知的環(huán)境中根據(jù)其自身的定位信息對(duì)地圖進(jìn)行增量式的更新，并使用 已創(chuàng)建的地圖進(jìn)行自我定位。圖 2.1 為同步定位和地圖構(gòu)建的過(guò)程示意圖。 圖 2.1同步定位和地圖構(gòu)建過(guò)程示意圖 SLAM 問(wèn)題可以定義為一個(gè)最大后驗(yàn)概率估計(jì)。記地圖為 m,t 時(shí)刻機(jī)器人的位姿為 xt= (xt,yt,t)T, 觀測(cè)信息為 zt, 控制信息為 ut, 記 Xt= x0,x1,.,xt, Zt= z0,z1,.,zt, Ut1= u0,u1,.,ut1. 在實(shí)際問(wèn)題中，機(jī)器人根據(jù)觀測(cè)信息及控制信息，求位姿信息的后驗(yàn)概率分布，即 Xt, Ep(Xt|Zt,Ut1,m), 通常簡(jiǎn)化為 xt, Ep(xt|Zt,Ut1,m) 根據(jù)貝葉斯估計(jì)理論，有： p(xt|Zt,Ut1,m) = p(zt|xt,Zt1,Ut1,m)p(xt|Zt1,Ut1,m)(2.1) 5 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 又因?yàn)?SLAM 問(wèn)題符合馬爾科夫性質(zhì)，及當(dāng)一個(gè)隨機(jī)過(guò)程在給定現(xiàn)在狀態(tài)及所有過(guò)去狀態(tài) 情況下，其未來(lái)狀態(tài)的條件概率分布僅依賴于當(dāng)前狀態(tài)，可得： p(xt|Zt,Ut1,m) = p(zt|xt,m)p(xt|Zt1,Ut1,m)(2.2) 將（2.2）式中 p(xt|Zt1,Ut1,m) 用全概率公式展開(kāi)，可得： p(xt|Zt,Ut1,m) = p(zt|xt,m) p(xt,xt1|Zt1,Ut1,m)dxt1(2.3) 對(duì)（2.3）式再用一次貝葉斯公式和馬爾科夫性質(zhì)簡(jiǎn)化，可得： p(xt|Zt,Ut1,m) = p(zt|xt,m) p(xt|xt1,ut1,m)p(xt1|Zt1,Ut1,m)dxt1(2.4) 其中 p(zt|xt,m) 為觀測(cè)模型，p(xt|xt1,ut1,m) 為運(yùn)動(dòng)模型。 這就是 SLAM 問(wèn)題的一般模型。 2.2SLAM ?題?題? 2.2.1? 基于濾波器的方法主要是利用了遞歸貝葉斯估計(jì)原理，在假定從 0 到 t 時(shí)刻的觀測(cè)信 息以及控制信息已知的條件下，對(duì)系統(tǒng)狀態(tài)（包括機(jī)器人當(dāng)前位姿以及所有地圖特征位置） 的后驗(yàn)概率進(jìn)行估計(jì)。根據(jù)后驗(yàn)概率表示方式的不同，存在多種基于濾波器的方法，常用 的包括擴(kuò)展卡爾曼濾波（EKF） 5、擴(kuò)展信息濾波（EIF）6、以及粒子濾波（PF）7 方法等。 為強(qiáng)調(diào)其增量式特性，基于濾波的 SLAM 方法通常被稱為在線 SLAM（on-line SLAM） 。 2.2.2? 與濾波方法中每一步只考慮機(jī)器人當(dāng)前位姿不同，基于平滑的方法通過(guò)所有的觀測(cè)信 息估計(jì)機(jī)器人完整的運(yùn)動(dòng)軌跡和地圖，因而也被稱為完全 SLAM 方法（full SLAM） 。由于 地圖特征可以通過(guò)邊緣化方法轉(zhuǎn)化為位姿間的約束，從而簡(jiǎn)化為位姿序列的估計(jì)。這類方 法可以用圖的方式作直觀描述，所得的圖被稱為位姿圖（pose graph） 。圖中的節(jié)點(diǎn)對(duì)應(yīng)機(jī) 器人在不同時(shí)刻的位置和姿態(tài)，而邊則描述了位姿與位姿間的空間約束關(guān)系。這種約束可 以通過(guò)里程計(jì)或觀測(cè)信息的配準(zhǔn)得到。在圖構(gòu)造好后，對(duì)圖中節(jié)點(diǎn)所處的位姿進(jìn)行優(yōu)化， 使其最好地滿足邊所表示的約束關(guān)系，優(yōu)化的結(jié)果即對(duì)應(yīng)機(jī)器人的運(yùn)動(dòng)軌跡。由于平滑方 法的這種直觀特性，又被稱為基于圖優(yōu)化的方法。過(guò)去，基于圖優(yōu)化的方法被認(rèn)為太耗費(fèi) 6 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 時(shí)間，無(wú)法滿足 SLAM 的實(shí)時(shí)性要求，但隨著高效求解方法的出現(xiàn)，基于圖優(yōu)化的方法重 新得到重視，在經(jīng)歷一個(gè)復(fù)興后，其成為當(dāng)前 SLAM 研究的一個(gè)熱點(diǎn)。 在第三章，我們將詳細(xì)介紹一種最具代表性的基于圖優(yōu)化的方法，g2o（General Framework for Graph Optimization） 8。 7 3? SLAM ? 3.1? SLAM ? 基于圖優(yōu)化的 SLAM 方法由 Lu 與 Milios9首先提出。通過(guò)保留所有的觀測(cè)數(shù)據(jù)幀以 及幀間的空間約束關(guān)系，并將約束看作是隨機(jī)觀測(cè)，然后采用最大似然方法估計(jì)機(jī)器人 的位姿。這種思想可以通過(guò)圖的方式形象地表現(xiàn)出來(lái)。用圖中的節(jié)點(diǎn)表示待求解的機(jī)器 人位姿，用節(jié)點(diǎn)間的邊來(lái)描述位姿間的空間約束關(guān)系，則對(duì)位姿序列的估計(jì)可轉(zhuǎn)化為圖 的優(yōu)化問(wèn)題，即通過(guò)調(diào)整圖中的節(jié)點(diǎn)的位置使其最好地滿足邊的約束關(guān)系。Gutmannn 和 Konolige10在 Lu 與 Milios 的基礎(chǔ)上提出了高效的閉環(huán)檢測(cè)及圖構(gòu)建方法，從而形成了基 于圖優(yōu)化的增量式 SLAM 算法框架，其主要包括順序配準(zhǔn)、環(huán)形閉合檢測(cè)以及圖優(yōu)化 3 大 部分，它們之間的關(guān)系可以通過(guò)下圖來(lái)表示。 圖 3.1基于圖優(yōu)化的 SLAM 框架 順序配準(zhǔn)主要考慮連續(xù)數(shù)據(jù)幀間的匹配及相對(duì)姿態(tài)估計(jì)問(wèn)題，而環(huán)形閉合檢測(cè)則主要 根據(jù)觀測(cè)數(shù)據(jù)判斷機(jī)器人是否處在之前已訪問(wèn)區(qū)域。這兩部分都與觀測(cè)數(shù)據(jù)密切相關(guān)。從 數(shù)據(jù)處理的觀點(diǎn)來(lái)看，兩者都是要解決數(shù)據(jù)關(guān)聯(lián)問(wèn)題，前者考慮局部數(shù)據(jù)關(guān)系，而后者則 處理全局?jǐn)?shù)據(jù)關(guān)系。從基于圖的表示上看，順序配準(zhǔn)與環(huán)形閉合檢測(cè)都是根據(jù)觀測(cè)信息建 立圖節(jié)點(diǎn)間的約束，即完成圖的構(gòu)建。兩者一起被稱為 SLAM 前端（SLAM front-end） 。 由于觀測(cè)噪聲以及配準(zhǔn)誤差的存在，通過(guò)觀測(cè)信息配準(zhǔn)得到的位姿圖往往不具備一致 性。圖中的邊與邊之間常常存在“沖突” 。舉例來(lái)說(shuō)，若用 Ti來(lái)表示數(shù)據(jù)幀間的相對(duì)變換 9 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 矩陣而且 T0,T1,.,Tn構(gòu)成一個(gè)閉環(huán)的話，從理論上講，必然存在 T0T1.Tn= I, 其中 I 為 單位矩陣。但通過(guò)觀測(cè)信息配準(zhǔn)得到的相對(duì)變換矩陣通常不滿足該理論的約束。在基于圖 的表示中，機(jī)器人的位姿被看作是隨機(jī)變量，位姿間的約束則是與隨機(jī)變量相關(guān)的觀測(cè)， 那么圖優(yōu)化結(jié)果則對(duì)應(yīng)于位姿的最大似然估計(jì)。與順序配準(zhǔn)及環(huán)形閉合檢測(cè)不同，圖優(yōu)化 部分一般不直接處理觀測(cè)數(shù)據(jù)，而是對(duì) SLAM 前端構(gòu)造的圖進(jìn)行優(yōu)化，因而，圖優(yōu)化通常 又被稱為 SLAM 后端（SLAM back-end）, 與 SLAM 前端相區(qū)分。 3.2? SLAM ? 在基于圖的 SLAM 中，機(jī)器人的位姿被圖中的節(jié)點(diǎn)描述。觀測(cè)值或者里程計(jì)測(cè)量值引 起的位姿之間的空間約束通過(guò)節(jié)點(diǎn)間的線段表示。更細(xì)節(jié)的是，一個(gè)基于圖的 SLAM 算法 構(gòu)建了一個(gè)不含原始傳感器測(cè)量值的圖。圖中的每一個(gè)節(jié)點(diǎn)代表一個(gè)機(jī)器人的位姿。節(jié)點(diǎn) 間的線代表兩個(gè)機(jī)器人位姿間的空間約束。一個(gè)約束包含兩點(diǎn)間相對(duì)轉(zhuǎn)移的可能性分布。 如下圖所示： 圖 3.2節(jié)點(diǎn)圖 圖中的節(jié)點(diǎn)表示機(jī)器人在某個(gè)時(shí)刻的位姿。鄰近的節(jié)點(diǎn)通過(guò)邊相連，這些邊是由于測(cè) 量值產(chǎn)生的兩個(gè)位姿之間的空間約束。兩個(gè)連續(xù)節(jié)點(diǎn)之間的邊 et1,t來(lái)自于里程計(jì)信息， 其它的邊是由各種傳感器對(duì)相同環(huán)境觀察而產(chǎn)生的空間約束。 一旦圖被構(gòu)建， 我們?nèi)ふ易钅軡M足約束的機(jī)器人位姿的結(jié)構(gòu)。 因此， 基于圖的 SLAM 10 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 問(wèn)題解耦為兩個(gè)任務(wù)：從原始測(cè)量值中構(gòu)建圖（圖構(gòu)建） ，通過(guò)圖中的線段，決定最可能的 位置結(jié)構(gòu)（圖優(yōu)化） 。 圖 3.3 取參數(shù)向量 x = (x1,.,xt)T, 這里 xi代表機(jī)器人在 i 時(shí)刻的位姿。zij和 ij分別作為 節(jié)點(diǎn) i 和節(jié)點(diǎn) j 間測(cè)量值的均值和測(cè)量值的信息矩陣。 zij(xi,xj) 作為節(jié)點(diǎn) zi和 xj之間的 期望測(cè)量值。所以，測(cè)量值 zij的對(duì)數(shù)似然函數(shù) Iij滿足以下形式 Iij zij zij(xi,xj)Tijzij zij(xi,xj)(3.1) 取 e(xi,xj) 作為期望的觀測(cè)值 zij和機(jī)器人真實(shí)的觀測(cè)值 zij的差。為了簡(jiǎn)化記號(hào)，我 們將這種測(cè)量值的標(biāo)號(hào)寫成誤差函數(shù) e(xi,xj) = zij zij(3.2) 最大似然方法的目標(biāo)是去找到節(jié)點(diǎn) x的配置，使得所有觀測(cè)值的負(fù) log 對(duì)數(shù)似然函數(shù) F（x）最小 F(x) = (i,j)C eT ijijeij (3.3) 即解決以下方程： x= argmin x F(x)(3.4) 接下來(lái)，我們可以采用 Gauss-Newton 或者 Levenberg-Marquardt 算法11處理這種非線 性最小二乘優(yōu)化問(wèn)題。 11 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 3.3? 如果對(duì)機(jī)器人的位姿有一個(gè)好的初始位置的估計(jì) x，那么能用 Gauss-Newton 或者 Levenberg-Marquardt 算法11求得方程（3.4）的數(shù)值解（該方法來(lái)源于參考文獻(xiàn) 8）主要 思想就是在初始估計(jì)附近通過(guò)一階泰勒級(jí)數(shù)展開(kāi)近似誤差函數(shù) ek= ( xk+ xk) = ek( x + x) = ek+ Jkx (3.5) 這里 Jk是 ek(x) 在 x 處的雅克比矩陣，把方程（3.5）代入方程（3.3） ，得到 Fk( x + x) = ek( x + x)Tkek( x + x) (ek+ Jkx)Tk(ek+ Jkx) = eT kkek+ 2e T kkJkx + x TJT kkJkx = ck+ 2bkx + xTHkx (3.6) 這樣，方程 F(x) 可以寫為 F( x + x) = (k)C Fk( x + x) kC ck+ 2bkx + xTHkx = c + 2bTx + xTHx (3.7) 這里，記 c = ck,b = bk,H = Hk. 為了使目標(biāo)函數(shù)最小，通過(guò)求導(dǎo)，可知 x 滿足 Hx= b(3.8) 通過(guò)解線性方程組，我們可以得到 x接下來(lái)，我們把 x加到初始估計(jì)值中，對(duì)初 始估計(jì)值進(jìn)行更新 x= x + x(3.9) 接下來(lái)進(jìn)行迭代，重新得到公式（3.7）的線性化結(jié)果，求解公式（3.8） ，更新公式 （3.9） 。在每次迭代中，上一次結(jié)果被當(dāng)做新的初始估計(jì)。 以上過(guò)程是一個(gè)通用的多元函數(shù)最小化的求解方法。然而，這種方法假設(shè)參數(shù) x 處于 歐氏空間。對(duì)于 SLAM 問(wèn)題，機(jī)器人的位置通常包括坐標(biāo)和姿態(tài)，所以位姿 x 處于非歐氏 空間。對(duì)此，我們引入流形12的方法去求解此問(wèn)題。 12 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 為了解決參數(shù)處于非歐氏空間問(wèn)題，通常的做法是把誤差最小化問(wèn)題放在流形中解決。 流形是一個(gè)數(shù)學(xué)空間，在全局范圍內(nèi)流形不一定是歐氏的，但是在局部可以被看做歐氏13。 在 SLAM 問(wèn)題中，每一個(gè)位姿 xi都包含一個(gè)坐標(biāo)向量 ti和一個(gè)旋轉(zhuǎn)向量 i, 坐標(biāo)向量 顯然屬于歐氏空間，而旋轉(zhuǎn)向量 i存在于非歐氏空間中。為了避免奇異，這些空間用旋轉(zhuǎn) 矩陣或者四元數(shù)表示。在流形空間中，定義一個(gè)操作符號(hào) ?，用來(lái)處理非歐氏空間中的運(yùn) 算 ek(xk) def. = ek( xk? xk) ek+ Jkx (3.10) 這里 x 是在初始位置 x 附近的微小增量。雅克比矩陣 Jk為 Jk= ek( x ? x) x |x=0(3.11) 同樣，經(jīng)過(guò)線性化后，我們能得到公式（3.8）同樣的結(jié)果, 求導(dǎo)后得到 Hx= b(3.12) 相應(yīng)的，對(duì)初始估計(jì)進(jìn)行更新 x= x ? x(3.13) 如此迭代下去，直至滿意的結(jié)果。 3.4?題?題? 一開(kāi)始，本文對(duì) 30 個(gè)節(jié)點(diǎn)進(jìn)行圖優(yōu)化處理，采用高斯牛頓方法迭代，時(shí)間復(fù)雜度為 O(n3), 計(jì)算機(jī)每次迭代的時(shí)間在 300ms 左右，可以接受。但當(dāng)本文使用公開(kāi)數(shù)據(jù)集測(cè)試算 法時(shí)，該官方數(shù)據(jù)集有 934 個(gè)節(jié)點(diǎn)，計(jì)算量爆炸，計(jì)算機(jī)過(guò)了相當(dāng)長(zhǎng)的時(shí)間都沒(méi)有給出計(jì) 算結(jié)果。 對(duì)此，本文想到了對(duì)整圖分解，局部?jī)?yōu)化的方法。首先將圖中的每 100 個(gè)連續(xù)節(jié)點(diǎn)分 為一個(gè)子圖，然后對(duì)子圖中的這 100 個(gè)節(jié)點(diǎn)進(jìn)行一次圖優(yōu)化，將每一個(gè)子圖優(yōu)化的結(jié)果拼 接起來(lái)作為最后的優(yōu)化結(jié)果。雖然該思想不能保證嚴(yán)格意義上的全局優(yōu)化，但是能夠使計(jì) 算量顯著下降，保證實(shí)時(shí)性。對(duì)這 934 個(gè)節(jié)點(diǎn)分為 10 組，最后每次迭代的時(shí)間在 500ms 左 右。優(yōu)化效果在下一節(jié)中展示。 13 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 3.5? 這里，我們以一個(gè) 2D SLAM 問(wèn)題，并使用 Intel Research Lab、ManhattanOlson 數(shù)據(jù)集 為例，展開(kāi)這一仿真過(guò)程。 3.5.1? 圖 3.42D SLAM 節(jié)點(diǎn)圖 圖 3.4 描述了一個(gè) SLAM 圖。機(jī)器人的位姿用節(jié)點(diǎn) xs t 表示，路標(biāo)用節(jié)點(diǎn) xl i 表示。我 們假設(shè)路標(biāo)只有位置信息而沒(méi)有方向信息，所以路標(biāo)屬于歐氏空間。機(jī)器人的位置既有 x y 坐標(biāo)，又包含旋角度 , 所以位姿屬于 SE(2) 空間。機(jī)器人在某一位時(shí)刻對(duì)路標(biāo)的測(cè) 量值用 zl 表示，連續(xù)節(jié)點(diǎn)間的里程計(jì)測(cè)量值用 z s 表示。 3.5.2? ? 在連續(xù)的兩個(gè)機(jī)器人位姿節(jié)點(diǎn) xs t 和 xs t+1中，存在的約束為里程計(jì)測(cè)量值 z s t,t+1。因?yàn)?傳感器誤差的存在，所以里程計(jì)測(cè)量值往往與真值有差距。我們假設(shè)噪聲是高斯白噪聲， 那么節(jié)點(diǎn) xs t 和 xs t+1之間的邊可以用以下兩個(gè)參數(shù)描述 14 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 1. zs t,t+1 SE(2) 代表連續(xù)節(jié)點(diǎn)間里程測(cè)量值 2. s t,t+1 33代表里程計(jì)測(cè)量值的協(xié)方差矩陣 ? 在機(jī)器人位姿節(jié)點(diǎn) xs t 與路標(biāo) xl i間也存在一種約束，這種約束來(lái)自觀測(cè)傳感器 z l t,i。同 樣，由于傳感器存在誤差，測(cè)量值與真值之間存在偏差。同樣假設(shè)這種噪聲是高斯白噪聲， 那么節(jié)點(diǎn) xs t 與路標(biāo) xl i間的約束可以用以下兩個(gè)參數(shù)描述 1. zl t,i 2代表在 xs t 對(duì)路標(biāo) xl i的觀測(cè)值 2. l t,i 22代表測(cè)量值的協(xié)方差矩陣 3.5.3? 構(gòu)建誤差函數(shù)常用的方法是根據(jù)初始估計(jì)的 xk，預(yù)測(cè)期望的觀測(cè)值 zk的，然后誤差 函數(shù)即為真實(shí)觀測(cè)值 zk與預(yù)測(cè)觀測(cè)值 zk的差。對(duì)于機(jī)器人節(jié)點(diǎn) xs t 與路標(biāo) xl i 之間約束誤 差，首先預(yù)測(cè)一個(gè)期望的測(cè)量值 hl t,i(x s t,x l i)，期望的測(cè)量值是指從機(jī)器人節(jié)點(diǎn) x s t 看路標(biāo) xl i 存在的位置，所以， zt,i(xs t,x l i) 形式如下 zt,i(xs t,x l i) = (xi xs t)cos s t + (yi ys t)sin s t (xi xs t)sin s t + (yi ys t)cos s t (3.14) 誤差為真實(shí)的測(cè)量值與預(yù)測(cè)的測(cè)量值之差，那么，節(jié)點(diǎn)與路標(biāo)之間的誤差函數(shù)如下 el t,i(x s t,x l i) = zt,i zt,i(x s t,x l i) (3.15) 同樣，我們可以定義機(jī)器人連續(xù)節(jié)點(diǎn) xs t 和 xs t+1 的里程計(jì)約束誤差。首先預(yù)測(cè)一個(gè)期 望的里程計(jì)數(shù)值， zt,t+1(xs t.x s t+1) = x s t+1 x s t (3.16) 誤差為真實(shí)的里程計(jì)測(cè)量值與預(yù)測(cè)的里程計(jì)測(cè)量值之差，那么連續(xù)節(jié)點(diǎn)間的誤差函數(shù) 如下 el t,t+1(x s t,x s t+1) = zt,t+1 zt,t+1(x s t.x s t+1) (3.17) 有了 (3.15) 和（3.17）兩個(gè)誤差函數(shù)，我們可以利用公式（3.7） （3.8） （3.9）迭代求解。 15 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 3.5.4? 下面我們以 Intel Research Lab 公開(kāi)數(shù)據(jù)集做一個(gè)數(shù)據(jù)處理 圖 3.5Intel Research Lab 該數(shù)據(jù)集中共有 934 個(gè)節(jié)點(diǎn)，2780 條約束，其中 934 條約束來(lái)自于里程計(jì)信息，即連 續(xù)機(jī)器人位姿間的約束；1837 條約束來(lái)自于觀測(cè)器信息，即非連續(xù)機(jī)器人位姿間的約束。 最開(kāi)始，我們將里程計(jì)信息作為初始估計(jì) x, 得到一張初始環(huán)境地圖，如圖 3.6 所示， 初始地圖大致滿足實(shí)驗(yàn)室環(huán)境，但中間走廊處效果較差。 圖 3.6初始環(huán)境地圖 16 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 然后我們通過(guò)誤差最小二乘的方法對(duì)以上數(shù)據(jù)進(jìn)行迭代優(yōu)化，得到以下的結(jié)果 圖 3.7一次迭代后的環(huán)境地圖 圖 3.8十次迭代后的環(huán)境地圖 圖 3.7 圖 3.8 分別是一次迭代和十次迭代后的結(jié)果。與初始估計(jì)對(duì)比可知，優(yōu)化后的 地圖結(jié)構(gòu)清晰，特別是地圖中間的走廊，經(jīng)過(guò)優(yōu)化后更加清晰明顯。由于里程計(jì)信息誤差 17 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 不大，所以收斂速度較快，一次迭代后就逼近最優(yōu)結(jié)果。 3.5.5? 我們使用 ManhattanOlson 數(shù)據(jù)集，該數(shù)據(jù)集中共有 3500 個(gè)節(jié)點(diǎn)，9098 條約束，其中 3500 條約束來(lái)自于里程計(jì)信息，即連續(xù)機(jī)器人位姿間的約束；5598 條約束來(lái)自于觀測(cè)器 信息，即非連續(xù)機(jī)器人位姿間的約束。 最開(kāi)始，我們將里程計(jì)信息作為初始估計(jì) x, 得到一張初始環(huán)境地圖，如圖 3.9 所示， 地圖右下角區(qū)域里程計(jì)誤差較大，地圖較為雜亂。 圖 3.9初始環(huán)境地圖 然后，我們通過(guò)誤差最小二乘的方法對(duì)以上數(shù)據(jù)進(jìn)行迭代優(yōu)化 圖 3.10一次迭代后的環(huán)境地圖 18 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 圖 3.11十次迭代后的環(huán)境地圖 圖 3.10 圖 3.11 分別是一次迭代和十次迭代后的結(jié)果。與初始估計(jì)對(duì)比可知，優(yōu)化后 的地圖結(jié)構(gòu)清晰，特別是地圖右下角較為復(fù)雜的地區(qū)，經(jīng)過(guò)優(yōu)化后結(jié)構(gòu)更加清晰明顯。 通過(guò) Intel Research Lab 數(shù)據(jù)和 ManhattanOlson 數(shù)據(jù)的仿真，可以看出基于最小二乘的 圖優(yōu)化方法具備較好的效果。 3.6? 這里，我們將圖優(yōu)化算法與光流法得到的二維地圖進(jìn)行一個(gè)比較。 光流法為實(shí)驗(yàn)室其他同學(xué)在做的項(xiàng)目，光流法基本原理為分析連續(xù)兩幀圖片光強(qiáng)度的 變化來(lái)判斷兩幀圖片的運(yùn)動(dòng)，進(jìn)而推算出攝像機(jī)的運(yùn)動(dòng)情況，從而生成攝像機(jī)的運(yùn)動(dòng)軌跡。 我們控制飛行器做一個(gè)規(guī)則的矩形運(yùn)動(dòng)，如圖 3.12 所示。 圖 3.12飛行器運(yùn)動(dòng)示意圖 19 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 圖 3.13 為光流法得到的機(jī)器人位姿信息，由于光流法存在測(cè)量誤差，所有最終結(jié)果有 一定的累積偏差。 圖 3.13光流法結(jié)果 接下來(lái)，我們要使用圖優(yōu)化方法對(duì)同樣的信息進(jìn)行處理。首先要構(gòu)建一張節(jié)點(diǎn)圖，初 始節(jié)點(diǎn)的位置我們使用光流法得到的結(jié)果，接下來(lái)我們要計(jì)算節(jié)點(diǎn)間的約束信息，這需要 一些圖像處理的技術(shù)。為了方便圖像處理，我們事先在地板上放置了一些綠色的方塊作為 路標(biāo)，如圖 3.14 中的綠色方塊所示。通過(guò) HSV 色域空間，我們很容易在圖像中識(shí)別出這 些路標(biāo)。如果我們?cè)趦蓭瑘D片中發(fā)現(xiàn)了相同的路標(biāo)，那么我們就很容易獲得這兩幀圖片所 在位置的空間約束。以圖 3.14 為例，假定圖像的中心點(diǎn)為飛行器的位置，在右圖中，飛行 器距綠色路標(biāo)為 a1, 在左圖中，飛行器距綠色路標(biāo)為 a2, 由于路標(biāo)位置固定，那么我們很容 易得到這兩個(gè)位姿間的約束為 a2 a1。 圖 3.14通過(guò)同一路標(biāo)計(jì)算位置關(guān)系 通過(guò)以上圖像處理的方法，我們能夠得到不同位姿間的約束關(guān)系，即節(jié)點(diǎn)圖中的邊， 這樣我們可以使用圖優(yōu)化來(lái)對(duì)所有位姿進(jìn)行優(yōu)化，優(yōu)化結(jié)果如圖 3.15 所示。 20 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 圖 3.15圖優(yōu)化結(jié)果 通過(guò)圖 3.13 與圖 3.15 對(duì)比可以看出，圖優(yōu)化得到的結(jié)果更加符合實(shí)際情況，圖優(yōu)化 方法具有良好的效果。 21 4? SLAM ? 4.1? 一個(gè)完整的圖優(yōu)化 SLAM 問(wèn)題分為前端和后端，前端負(fù)責(zé)處理傳感器數(shù)據(jù)，得到 初始的位姿估計(jì)及約束，后端對(duì)位姿進(jìn)行圖優(yōu)化。這里，我們前端借助于 LSD-SLAM （Large-Scale Direct Monocular SLAM）方法14，后端使用上文介紹過(guò)的基于最小二乘的圖 優(yōu)化算法。如圖 4.1 所示，攝像頭獲取數(shù)據(jù)，然后由 LSD-SLAM 處理生成節(jié)點(diǎn)圖，節(jié)點(diǎn)圖 由 Graph-based Optimation(即圖優(yōu)化) 處理。 圖 4.1整體架構(gòu) LSD-SLAM 是一種單目視覺(jué) SLAM 方法15，該方法直接進(jìn)行連續(xù)圖像匹配獲得當(dāng)前 環(huán)境的深度信息圖，并選取關(guān)鍵幀，通過(guò)連續(xù)圖像與關(guān)鍵幀的匹配得到攝像頭的運(yùn)動(dòng)軌跡， 如圖 4.2 所示。該方法選取圖像上所有的點(diǎn)進(jìn)行比較，而不是僅僅選取角點(diǎn)，所以得到的 深度圖相比角點(diǎn)方法信息豐富。這里直接使用該算法的開(kāi)源程序，對(duì)此算法不再展開(kāi)詳細(xì) 論述。 23 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 圖 4.2LSD-SLAM 對(duì)于后端，我們使用基于圖優(yōu)化的 g2o 方法8，處理前端數(shù)據(jù)，得到滿足全局誤差最 小的環(huán)境地圖。 本次畢業(yè)設(shè)計(jì)使用 ROS（機(jī)器人操作系統(tǒng)，Robot Operating System）平臺(tái)（4.1.3 節(jié)將 對(duì)此平臺(tái)進(jìn)行介紹） ，在 ROS 平臺(tái)下將飛行器控制程序、LSD_SLAM、圖優(yōu)化程序整合起 來(lái), 如圖 4.3 所示。 圖 4.3ROS 程序架構(gòu) 4.2? 4.2.1? 由于進(jìn)行了手持和機(jī)載兩種試驗(yàn)，所以選取了兩種攝像頭。一種是普通 USB 攝像頭， 羅技 C270USB；另外一種是 AR.Drone 飛行器上的前置攝像頭。 24 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 圖 4.4羅技攝像頭 圖 4.5AR.Drone 飛行器前置攝像頭 首先要分別對(duì)圖 4.4、圖 4.5 這兩款攝像頭進(jìn)行標(biāo)定，拿到相機(jī)內(nèi)參數(shù)，去除畸變。這 里，我們使用 ROS 平臺(tái) (Robot Operating System) 下的 camera_calibration 包對(duì)攝像頭進(jìn)行 校正。通過(guò)攝像頭對(duì)棋盤格不同角度的拍攝，獲取攝像頭內(nèi)參數(shù)。如圖 4.6 所示。 25 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 圖 4.6攝像頭標(biāo)定過(guò)程 得到攝像機(jī)的內(nèi)參數(shù)矩陣，下表為 AR.Drone 飛行器前置攝像頭內(nèi)參數(shù)： image width: 640 image height: 360 camera matrix: rows: 3 cols: 3 data: 561.126855, 0, 344.468073, 0, 558.137502, 163.498467, 0, 0, 1 distortion model: plumb bob distortion coefficients: rows: 1 cols: 5 data: -0.518857, 0.279803, 0.001078, -0.002978, 0 rectification matrix: rows: 3 cols: 3 data: 1, 0, 0, 0, 1, 0, 0, 0, 1 projection matrix: rows: 3 cols: 4 data: 455.355591, 0, 350.400233, 0, 0, 523.73053, 161.213323, 0, 0, 0, 1, 0 26 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 圖 4.7矯正前后攝像頭圖像對(duì)比 圖 4.7 為矯正前后攝像圖頭像對(duì)比圖，上圖為矯正之前攝像頭得到的圖像，下圖為矯 正之后攝像頭得到的圖像。攝像頭矯正結(jié)束后，我們就可以使用 LSD-SLAM 對(duì)圖像進(jìn)行 處理。 4.2.2? 飛行器使用歐洲 Parrot 公司生產(chǎn)的 AR.Drone2.0 飛行器，該飛行器在 LINUX 下有一套 完善的開(kāi)發(fā)平臺(tái)，方便開(kāi)發(fā)者使用該飛行器做各種應(yīng)用的開(kāi)發(fā)。同時(shí)該飛行器還自帶光流 懸停算法，為室內(nèi)地圖構(gòu)建提供了一個(gè)可靠的平臺(tái)。 AR.Drone2.0 飛行器與計(jì)算機(jī)以網(wǎng)絡(luò)的方式進(jìn)行通信，所有的計(jì)算程序在計(jì)算機(jī)上運(yùn) 行，計(jì)算機(jī)再發(fā)送控制指令返回到飛行器上。飛行器把攝像頭讀到的圖像通過(guò) wifi 發(fā)送給 計(jì)算機(jī)，在計(jì)算機(jī)上進(jìn)行地圖構(gòu)建。 27 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 圖 4.8AR.Drone 飛行器 4.2.3ROS ? ROS（機(jī)器人操作系統(tǒng)，Robot Operating System） ，是專為機(jī)器人軟件開(kāi)發(fā)所設(shè)計(jì)出來(lái) 的一套電腦操作系統(tǒng)架構(gòu)。這里，我們對(duì)飛行器的操作、視頻的讀取、LSD-SLAM 算法的 使用以及圖優(yōu)化均在 ROS 平臺(tái)下完成。 ROS 系統(tǒng)的一大特點(diǎn)是具有很好的模塊化設(shè)計(jì)功能，它通過(guò)將各個(gè)模塊封裝為節(jié)點(diǎn)， 并在節(jié)點(diǎn)之間使用話題或者服務(wù)的方式進(jìn)行通信，達(dá)到獨(dú)立各個(gè)模塊而又整合所有功能的 目的。 圖 4.9ROS topic 通信方式 圖 4.9，可以很好的說(shuō)明 ROS 系統(tǒng)的信息交互方式，圖中 ardrone_drive 和 key- board_contorller 為兩個(gè)互相獨(dú)立的節(jié)點(diǎn)，從 ardrone_drive 到 keyboard_contorller 的信息通 過(guò) ardrone/image_raw 這一話題傳遞。從 keyboard_contorller 到 ardrone_drive 的信息通過(guò) ardrone/land 這一話題傳遞。這樣實(shí)現(xiàn)了兩個(gè)節(jié)點(diǎn)既能獨(dú)立工作，又能相互傳遞信息。 28 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 4.3? 整個(gè)軟件程序框架如圖 4.10 所示，攝像頭得到的圖片首先進(jìn)行追蹤，然后判斷是否需 要選取新的關(guān)鍵幀，將當(dāng)前幀加入到地圖當(dāng)中，最后用 g2o 去優(yōu)化處理。 圖 4.10基于圖優(yōu)化的單目視覺(jué) slam 流程圖 4.4? 4.4.1? 選取實(shí)驗(yàn)室桌面環(huán)境，如圖 4.11 所示，我們手持?jǐn)z像頭，對(duì)桌面物體進(jìn)行實(shí)時(shí)地圖構(gòu) 建。這里攝像頭圍繞桌面物體均勻移動(dòng)。該桌面上放有書本、紙巾、水杯等雜物，特點(diǎn)清 晰，層次鮮明，有利于基于視覺(jué)的地圖構(gòu)建。 29 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 圖 4.11實(shí)驗(yàn)室桌面 圖 4.12 為攝像頭拍攝并處理后的畫面，該圖像經(jīng)過(guò)去畸變矯正處理，并且被轉(zhuǎn)化為黑 白圖像，以減少計(jì)算量。 圖 4.12攝像頭拍攝處理后圖像 30 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 圖 4.13實(shí)驗(yàn)室桌面構(gòu)建過(guò)程 圖 4.13 為實(shí)驗(yàn)室桌面地圖構(gòu)建過(guò)程，紅色方框?yàn)閿z像頭當(dāng)前時(shí)刻位姿，藍(lán)色方框?yàn)閿z 像機(jī)過(guò)去時(shí)刻位姿，每一個(gè)方框代表一個(gè)關(guān)鍵幀。當(dāng)攝像頭運(yùn)動(dòng)超過(guò)一定距離后，增添一 個(gè)新的關(guān)鍵幀。每增添一個(gè)關(guān)鍵幀，對(duì)采集到的數(shù)據(jù)進(jìn)行一次全局優(yōu)化。 圖 4.14桌面地圖及攝像頭運(yùn)動(dòng)軌跡 1 31 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 圖 4.15桌面地圖及攝像頭運(yùn)動(dòng)軌跡 2 實(shí)驗(yàn)視頻連接：/v_show/id_XOTUzODU1MDU2.html 得到的三維地圖較好的符合實(shí)際情況。 4.4.2? 我們使用機(jī)載攝像頭，操縱 AR.Drone 飛行器，對(duì)智控所一樓大廳中的物體進(jìn)行拍攝 和地圖構(gòu)建。在實(shí)驗(yàn)的過(guò)程中，保持飛行器的飛行穩(wěn)定。我們選取了特點(diǎn)較為鮮明的階梯 狀物體進(jìn)行地圖構(gòu)建。 圖 4.16智控所 1 樓大廳 32 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 圖 4.17AR.Drone 拍攝處理后圖像 圖 4.17 為 AR.Drone 前置攝像頭處理后的畫面，原始圖像通過(guò) wifi 傳到地面站，傳輸 頻率為 30Hz。在地面站此圖像進(jìn)行去畸變校正和黑白化處理，然后進(jìn)行基于圖的 SLAM 運(yùn)算。 圖 4.18智控所大廳物體構(gòu)建過(guò)程 33 浙江大學(xué)本科生畢業(yè)論文基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建 圖 4.18 為智控所局部大廳地圖構(gòu)建過(guò)程，紅色方框?yàn)閿z像頭當(dāng)前時(shí)刻位姿，藍(lán)色方框 為攝像機(jī)過(guò)去時(shí)刻位姿，每一個(gè)方框代表一個(gè)關(guān)鍵幀。當(dāng)攝像頭運(yùn)動(dòng)超過(guò)一定距離后，增 添一個(gè)新的關(guān)鍵幀。每增添一個(gè)關(guān)鍵幀，對(duì)采集到的數(shù)據(jù)進(jìn)行一次全局優(yōu)化。 圖 4.19智控所大廳局部地圖及攝像頭運(yùn)動(dòng)軌跡 1 圖 4.20智控所大廳局部地圖及攝像頭運(yùn)動(dòng)軌跡 2 實(shí)驗(yàn)視頻鏈接：/v_show/id_XOTUzODY1MTY4.html 34 基于飛行器平臺(tái)的室內(nèi)地圖構(gòu)建浙江大學(xué)本科生畢業(yè)論文 圖 4.19、圖 4.20 為兩個(gè)不同的角度觀看模型地圖。該地圖不僅畫出了攝像頭移動(dòng)的軌 跡，還基本復(fù)原了原始圖像，圖中階梯明顯，深度信息