基于飛行器平臺的室內地圖構建

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