基于視覺—慣性的四旋翼無人機自主導航技術的研究

何玲

摘 要 隨著科技的飛快發展，無人機的應用場合日益增多，同時也給人類帶來諸多便利。文章著力于四旋翼無人機自主導航技術的初步研究，結合國內外對無人機自主導航的研究提出常見的研究問題和方法。

關鍵詞 自主導航；定位；SLAM；路徑規劃

中圖分類號 TP3 文獻標識碼 A 文章編號 1674-6708（2018）221-0133-04

1 研究意義

隨著科技的進步，無人機逐漸走入人們的生活。無人機可以被快遞公司用于主站到分站貨物運輸工作。無人機也可承擔高速公路或城市擁堵道路的監控和疏導工作。攜帶攝像機的無人機的出現，給影視行業提供了極大便利的同時也大大降低了制作成本，也使本無法企及的復雜危險環境的拍攝成為可能，比如活躍火山口的拍攝。裝上激光測距或雙目攝像頭等具有深度信息的無人機甚至可以完成大型建筑、山脈的三維掃描及測量。

然而，無人機很難操控，即使專業的飛手也經常因失誤而出現墜機的情況。因此，排除人為因素，研究出安全穩定的自主飛行無人機成為人們的一致追求。當前主流自主導航方法是GPS—慣性系統。這在多數情況下是可行的，但在室內GPS沒有信號，因此無法導航。本課題擬使用視覺—慣性系統組合的導航方式，輔以超聲波測距對距離矢量的修正，從而實現較高精度的自主飛行。

2 國內外研究現狀

在過去的幾年里，微型飛行器激起了人們對無人機自主導航的極大興趣。美國賓夕法尼亞大學的GRASP實驗室首次利用四旋翼無人機成功實現室內躲避障礙物的自主導航飛行實驗。GRASP實現利用深度像機和激光測距儀實時構建三維空間地圖來引導四旋翼無人機在室內導航、翻轉及側向飛行通過狹小的窗口。在平衡控制上，該實驗室成功控制了飛行中的四旋翼無人機上直立木棍的平衡。而在多機配合飛行控制上，該實驗室實現了機群跟隨音樂進行編隊飛行、協同飛行等形式的表演。

2012年，德國慕尼黑工業大學Jakob Engel等人利用單目視覺SLAM技術，融合IMU及氣壓計數據，成功實現四旋翼無人機固定軌跡的飛行[1]，這也是首次在廉價微型旋翼無人機上實現的自主飛行實驗。

2013年，清華大學在國際空中機器人大賽（IARC）第六代比賽中獲得冠軍。該校使用一家德國公司生產名為Pelican的四軸飛行器，越過已經成熟的飛行器硬件及飛控系統的開發工作，將主要精力集中在室內自主導航的研究上。該團隊利用機載處理器采集前向攝像機、底部攝像機及激光測距儀等傳感器的數據，通過無線網絡傳輸到地面控制站，并由地面控制站進行數據融合、濾波、構圖、預測并生成最終設定時間點的導航控制命令，再次通過無線網絡傳輸回傳到無人機，進而實現導航。

2015年，復旦大學在大疆公司的Matrice 100無人機上為停車場管理者打造出了一款簡單易用、意義非凡的智能城市解決方案。該系統可以幫助交警或其他管理者識別違章停放的車輛。無人機沿著事先設定好的航線上飛行并通過機載攝像機實時識別每一輛汽車。該系統內置英特爾技術，可以識別車輛的停放位置及方向，從而檢測出車輛是否按規定停好。如果發現了違章停放的車輛，無人機將拍下該車輛車牌照片，并傳回地面控制中心。

3 常見研究點與經典方法

一般情況下，要想在未知環境中實現導航，必須具備這些能力：構建地圖、地圖定位和運動控制。這三大要素可以解決導航的4個基本問題： “世界看起來是什么樣子的？”“我在哪里？”“我要去哪里？”和“我要怎么去那里？”?！笆澜缈雌饋硎鞘裁礃幼拥?？”和“我在哪里？”由SLAM（Simultaneous Localization and Mapping，同步定位與構圖）解決。而“我要去哪里？”和“我要怎么去那里？”則被歸納到路徑規劃中完成。

3.1 同步定位與構圖

同步定位與構圖（SLAM）并不是單個的算法，而是一個系統框架，定義了一個寬泛的思想性的概念。研究者需要利用不止一種算法并相互配合來完成。SLAM系統主要有4個部分組成：問題建模、傳感器數據、地圖描述以及回環檢測，下面將分別對其介紹。

3.1.1 問題建模

SLAM 研究源起于Smith1986年發表的空間不確定性估計與表達[2]一文。在該論文中，定位問題是指機器人的運動軌跡，地圖則由一系列路標點構成。該文所述方法是基于濾波的。這類基于濾波的方法一直在早期的 SLAM 中占據主導地位，例如SIFT特征的提出者Lowe曾用EKF和SIFT實現了視覺SLAM[3]，最早的實時視覺SLAM系統即是基于EKF[4]。而后，粒子濾波器，FastSLAM以及基于優化的方法才逐漸興起。這個建模的過程，通常稱為SLAM 的基礎理論。由于傳感器和慣性測量設備均存在誤差，得到的軌跡與地圖通常與真實情況存在著一定的偏差。如何減少噪聲帶來的影響，建立準確、一致的地圖，是 SLAM 研究者們關心的主要問題。

3.1.2 傳感器數據處理

目前，SLAM研究中使用的傳感器包括激光測距儀、單目相機、立體相機、深度相機、聲吶以及多種傳感器信息融合。SLAM的求解方式與傳感器有著重要的關聯，因為從本質上說，SLAM所有的計算都是對傳感器數據的處理。通常來說，傳感器的選擇不同，SLAM基本方程的參數化會有很大的差別。如基于激光傳感器的 SLAM 一般使用EKF實現，基于相機的則用姿態圖（Pose Graph）進行優化，這是由于不同傳感器的特性不同而造成的，如表1。

3.1.3 地圖描述

建圖的問題則主要取決于地圖的類型。不同類型的地圖各有側重點，應用于不同的場合。主要有以下幾種類型。

1）度量地圖（Metric Map）。度量地圖利用物體間的精確位置關系來構圖。度量地圖的具體形式可分為稀疏形式（Sparse Map）與密集形式（Dense Map）。稀疏形式主要指由路標組成的地圖，而密集形式主要指占據網格地圖（Occupancy Grid Map）。稀疏路標圖主要用于早期研究，由于計算性能的限制，機器人的運動非常緩慢，地圖的規模也相對較小。目前常用的是密集型的占據網格地圖。通常把地圖按照某種分辨率分割成許多個小塊，以矩陣（對應二維情形）或八叉樹（對應三維情形）來表示。度量地圖簡單易于理解，但每一個網格點的狀態都需要保存，因此會消耗非常大的存儲空間。

2）拓撲地圖（Topological Map）。與度量地圖的精確性相比，拓撲地圖的表達角度完全不同。它采用由頂點和邊組成的圖（Graph）：G={V，E}的方式表達，因而更強調地圖元素的獨立性，以及元素之間的連通關系。其中頂點表示特征點的姿態，邊表示運動。拓撲地圖是一種比較緊湊的地圖表達方式。它可以放松地圖對精確位置的需要，去掉了地圖的很多細節問題。但是，拓撲地圖很難表達具有復雜結構的環境。拓撲地圖下一步研究的主要方向是對地圖進行分割形成結點與邊的問題。使用拓撲地圖進行導航與路徑規劃也是后續需要解決的重要一環。

3）混合地圖（Hybrid Map）。由于上述地圖表達方式各有優劣，因而有研究者認為應該構建帶有層次模型的地圖，混合使用不同的表達方式來處理地圖。其核心思想是：用度量地圖表達局部結構，用拓撲地圖表達各個小地圖之間的連通關系。此類方法有成功的案例，但由于技術復雜，不易于推廣。

3.1.4 回環檢測

回環檢測（Loop Closure Detection），是指機器人識別是否曾經到過某個地方。如果檢測成功，將進行環路融合計算，找出一個最優的更加接近真實的位置，從而顯著地減小累積誤差。

回環檢測本質上是通過檢測觀測數據相似性來實現的一種算法。對于視覺 SLAM，有的研究者使用傳統的模式識別的方法，把回環檢測建構成一個分類問題，訓練分類器進行分類。但多數的SLAM使用詞袋模型（Bag-of-Words）[5]。詞袋模型通過機器學習對從周圍環境里大量圖像提取的視覺特征進行聚類，利用特征類別建立較為精簡的詞典，然后將每幅圖都用詞典中的單詞進行表達?；丨h檢測的過程就是將當前的圖像中的單詞與歷史圖像中的單詞進行比較，相似度達到指定的閾值即視為回環檢測成功。

然而，錯誤的回環檢測結果可能會使地圖變得更糟糕。該錯誤分為兩類：一類是假陽性（False Positive），也稱作感知偏差（Perceptual Aliasing），即實際上不同的場景被歸為同一場景；另一類是假陰性（False Negative），又稱感知變異（Perceptual Variability），即事實上同一個場景被分成了兩個不同場景。感知偏差將使地圖質量變差，是希望避免的。盡量多地檢測出真實回環是檢驗回環檢測算法的重要標準。通常用準確率-召回率曲線來量化一個檢測算法的好壞。

3.2 路徑規劃（Path Planning）

路徑規劃是自主移動機器人的一個非常重要的組成部分。對于機器人來說，只有地圖還不行，它需要知道在地圖上如何實現從起始位置到達目標位置。路徑規劃就是在地圖已經存在的前提下，按照一定的標準要求，在充滿障礙的環境中尋找從起始狀態到達目標狀態的沒有碰撞的符合要求的路徑。

隨著不同領域對移動機器人的廣泛需求，越來越多的研究人員投入到了路徑規劃領域的研究，進而提出很多有效的路徑規劃算法。比如概率路線圖法、快速搜索隨機樹法等等。

3.2.1 概率路線圖

概率路線圖（Probabilistic RoadMaps，PRM）算法[6]是在1996年由Lydia Kavraki、JeanClaude Latombe提出的。概率路線圖按處理流程的先后分學習和查詢兩個階段。學習階段構建路線圖，查詢階段則是在構建好的路線圖中找出合適的路線。學習階段在位姿空間中概率隨機地采樣，并將位于自由位姿空間中，即無碰撞的采樣點連線成邊，由無碰撞的采樣點和邊共同組成了一張較為復雜的隨機路線圖。查詢階段將初始點和目標點分別連接到前一階段生成好的路線圖中，然后使用局部規劃器，在路線圖中隨機查找連接初始點和目標點的連接路徑。當初始點和目標點在同一個連通分支內時，一種可行的路徑很大幾率能被查找到。當初始點和目標點不在同一個連通分支內時，則回到學習階段，增加更多的采樣點，從而增加路徑聯通的可能性，再次進入查詢階段進行路徑搜索。循環進行這兩個階段的操作，一直到路徑被找到為止。

與之前的算法相比，概率路線圖克服了局部極小問題，且計算量小，適合多自由度的機器人路徑規劃。但是當環境中有窄道時，非常有可能出現采樣率低的現象，導致無法找到合適的路徑。

3.2.2 快速搜索隨機樹

快速搜索隨機樹（Rapidly-Exploring Random Tree，RRT）算法[7]由Steven M. Lavalle在1998年提出。該方法與概率路線圖（PRM）不同，它不需要PRM學習階段那樣先要對整個空間建模，能夠有效地解決高維空間和復雜約束條件下的路徑規劃問題。RRT算法是基于最優控制理論思想建立的，從起始狀態開始通過應用控制輸入，在最短時間內遞增地生長一棵搜索樹到達新的狀態。樹上的頂點表示狀態，有向邊表示控制輸入?？刂戚斎胱饔迷谙惹暗臓顟B從而得到新的狀態。當一個頂點到達目標狀態或期望的目標區域時，路徑搜索成功?？焖偎阉麟S機樹算法引入微分約束，以使規劃的路徑更加平滑，更適合機器人行進，也更具合理性?？焖偬剿麟S機樹算法被廣泛用在復雜的工作環境、存在微分約束的環境及高維狀態空間中。

快速搜索隨機數算法仍然不夠完美，當出現較多狹窄空間時，會導致收斂特別慢。有人采用雙向搜索技術，從起始狀態和目標狀態同時相向地增長搜索樹，從而提高搜索效率。也有人通過增加導向因子來引導節點擴展方向，從而使搜索樹有目標地生長，而不是漫無目標，這大大縮短了生長時間。

3.2.3 RRT*

雖然RRT算法具有運算速度快的優點，并且能在較短時間內找到解，但是由于RRT算法隨機性很大，已經證明是無法找到最優解的。為了解決這個問題，Karaman和Frazzol在 RRT 算法基礎上，引入節點選擇機制，提出了RRT*（RRT Star）算法[8]，這種算法雖然同樣無法找到最優解，但是可以保證找到一個接近最優的解。

4 預期創新點

4.1 地圖創新點

基于特征的SLAM一般會因為計算效率的問題而采用關鍵幀的方法，因而生成的地圖一般是稀疏的點云地圖。在稀疏點云地圖上生成的占據柵格地圖，一般在復雜的環境中不足以實現無碰撞導航。如何能在保證計算效率（實時）的前提下，保證復雜環境的無碰撞導航是本文要實現的目標之一。

4.2 路徑規劃創新點

基于RRT*路徑規劃算法獲得的導航路徑是一個漸進最優的圖，包含多個頂點和有向邊。每個頂點為無人機姿態，有向邊為控制輸入。實現導航就是讓無人機根據有向邊的控制輸入，達到每個頂點的姿態，最終到達目標區域。事實RRT*給出的路徑圖并不是平滑的，是一個個離散狀態的集合，連接兩個頂點的邊是直線，即只對應一個輸入，因此無人機很難無偏跟隨這條導航路徑。在無人機輸入極限的范圍內，如何優化（平滑）這條導航路徑，使其可以無偏跟蹤是本文要實現的另一個目標。

參考文獻

[1]Jakob Engel， Jürgen Sturm， and Daniel Cremers， “Camera-Based Navigation of a Low-Cost Quadrocopter，” in Intelligent Robots and Systems（IROS）， 2012.

[2]Smith R C， Cheeseman P， “On the representation and estimation of spatial uncertainty，” in International Journal of Robotics Research， 1986.

[3]Se S， Lowe D， and Little J， “Mobile robot localization and mapping with uncertainty using scale-invariant visual landmarks，” in International Journal of Robotics Research， 2002.

[4]Davison A， Reid I， Molton N， et al.， “Monoslam： Real-time single camera SLAM，”in IEEE Transactions on Pattern Analysis and Machine Intelligence， 2007.

[5]Filliat D， “A visual bag of words method for interactive qualitative localization and mapping，”in IEEE International Conference on Robotics and Automation （ICRA）， 2007.

[6]Kavraki L E et al.， “Probabilistic roadmaps for path planning in high-dimensional configuration spaces，” in IEEE Transactions on Robotics and Automation， 1996.

[7]Steven M. Lavalle， “Rapidly-exploring random trees： A new tool for path planning，” in Technical Report No. 98-11， 1998.

[8]Sertac Karaman， Emilio Frazzoli，”Sampling-based Algorithms for Optimal Motion Planning，” in The International Journal of Robotics Research， 2011.