基于視覺(jué)—慣性的四旋翼無(wú)人機(jī)自主導(dǎo)航技術(shù)的研究

何玲

摘 要 隨著科技的飛快發(fā)展，無(wú)人機(jī)的應(yīng)用場(chǎng)合日益增多，同時(shí)也給人類(lèi)帶來(lái)諸多便利。文章著力于四旋翼無(wú)人機(jī)自主導(dǎo)航技術(shù)的初步研究，結(jié)合國(guó)內(nèi)外對(duì)無(wú)人機(jī)自主導(dǎo)航的研究提出常見(jiàn)的研究問(wèn)題和方法。

關(guān)鍵詞 自主導(dǎo)航；定位；SLAM；路徑規(guī)劃

中圖分類(lèi)號(hào) TP3 文獻(xiàn)標(biāo)識(shí)碼 A 文章編號(hào) 1674-6708（2018）221-0133-04

1 研究意義

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

然而，無(wú)人機(jī)很難操控，即使專(zhuān)業(yè)的飛手也經(jīng)常因失誤而出現(xiàn)墜機(jī)的情況。因此，排除人為因素，研究出安全穩(wěn)定的自主飛行無(wú)人機(jī)成為人們的一致追求。當(dāng)前主流自主導(dǎo)航方法是GPS—慣性系統(tǒng)。這在多數(shù)情況下是可行的，但在室內(nèi)GPS沒(méi)有信號(hào)，因此無(wú)法導(dǎo)航。本課題擬使用視覺(jué)—慣性系統(tǒng)組合的導(dǎo)航方式，輔以超聲波測(cè)距對(duì)距離矢量的修正，從而實(shí)現(xiàn)較高精度的自主飛行。

2 國(guó)內(nèi)外研究現(xiàn)狀

在過(guò)去的幾年里，微型飛行器激起了人們對(duì)無(wú)人機(jī)自主導(dǎo)航的極大興趣。美國(guó)賓夕法尼亞大學(xué)的GRASP實(shí)驗(yàn)室首次利用四旋翼無(wú)人機(jī)成功實(shí)現(xiàn)室內(nèi)躲避障礙物的自主導(dǎo)航飛行實(shí)驗(yàn)。GRASP實(shí)現(xiàn)利用深度像機(jī)和激光測(cè)距儀實(shí)時(shí)構(gòu)建三維空間地圖來(lái)引導(dǎo)四旋翼無(wú)人機(jī)在室內(nèi)導(dǎo)航、翻轉(zhuǎn)及側(cè)向飛行通過(guò)狹小的窗口。在平衡控制上，該實(shí)驗(yàn)室成功控制了飛行中的四旋翼無(wú)人機(jī)上直立木棍的平衡。而在多機(jī)配合飛行控制上，該實(shí)驗(yàn)室實(shí)現(xiàn)了機(jī)群跟隨音樂(lè)進(jìn)行編隊(duì)飛行、協(xié)同飛行等形式的表演。

2012年，德國(guó)慕尼黑工業(yè)大學(xué)Jakob Engel等人利用單目視覺(jué)SLAM技術(shù)，融合IMU及氣壓計(jì)數(shù)據(jù)，成功實(shí)現(xiàn)四旋翼無(wú)人機(jī)固定軌跡的飛行[1]，這也是首次在廉價(jià)微型旋翼無(wú)人機(jī)上實(shí)現(xiàn)的自主飛行實(shí)驗(yàn)。

2013年，清華大學(xué)在國(guó)際空中機(jī)器人大賽（IARC）第六代比賽中獲得冠軍。該校使用一家德國(guó)公司生產(chǎn)名為Pelican的四軸飛行器，越過(guò)已經(jīng)成熟的飛行器硬件及飛控系統(tǒng)的開(kāi)發(fā)工作，將主要精力集中在室內(nèi)自主導(dǎo)航的研究上。該團(tuán)隊(duì)利用機(jī)載處理器采集前向攝像機(jī)、底部攝像機(jī)及激光測(cè)距儀等傳感器的數(shù)據(jù)，通過(guò)無(wú)線網(wǎng)絡(luò)傳輸?shù)降孛婵刂普?，并由地面控制站進(jìn)行數(shù)據(jù)融合、濾波、構(gòu)圖、預(yù)測(cè)并生成最終設(shè)定時(shí)間點(diǎn)的導(dǎo)航控制命令，再次通過(guò)無(wú)線網(wǎng)絡(luò)傳輸回傳到無(wú)人機(jī)，進(jìn)而實(shí)現(xiàn)導(dǎo)航。

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

3 常見(jiàn)研究點(diǎn)與經(jīng)典方法

一般情況下，要想在未知環(huán)境中實(shí)現(xiàn)導(dǎo)航，必須具備這些能力：構(gòu)建地圖、地圖定位和運(yùn)動(dòng)控制。這三大要素可以解決導(dǎo)航的4個(gè)基本問(wèn)題： “世界看起來(lái)是什么樣子的？”“我在哪里？”“我要去哪里？”和“我要怎么去那里？”。“世界看起來(lái)是什么樣子的？”和“我在哪里？”由SLAM（Simultaneous Localization and Mapping，同步定位與構(gòu)圖）解決。而“我要去哪里？”和“我要怎么去那里？”則被歸納到路徑規(guī)劃中完成。

3.1 同步定位與構(gòu)圖

同步定位與構(gòu)圖（SLAM）并不是單個(gè)的算法，而是一個(gè)系統(tǒng)框架，定義了一個(gè)寬泛的思想性的概念。研究者需要利用不止一種算法并相互配合來(lái)完成。SLAM系統(tǒng)主要有4個(gè)部分組成：?jiǎn)栴}建模、傳感器數(shù)據(jù)、地圖描述以及回環(huán)檢測(cè)，下面將分別對(duì)其介紹。

3.1.1 問(wèn)題建模

SLAM 研究源起于Smith1986年發(fā)表的空間不確定性估計(jì)與表達(dá)[2]一文。在該論文中，定位問(wèn)題是指機(jī)器人的運(yùn)動(dòng)軌跡，地圖則由一系列路標(biāo)點(diǎn)構(gòu)成。該文所述方法是基于濾波的。這類(lèi)基于濾波的方法一直在早期的 SLAM 中占據(jù)主導(dǎo)地位，例如SIFT特征的提出者Lowe曾用EKF和SIFT實(shí)現(xiàn)了視覺(jué)SLAM[3]，最早的實(shí)時(shí)視覺(jué)SLAM系統(tǒng)即是基于EKF[4]。而后，粒子濾波器，F(xiàn)astSLAM以及基于優(yōu)化的方法才逐漸興起。這個(gè)建模的過(guò)程，通常稱(chēng)為SLAM 的基礎(chǔ)理論。由于傳感器和慣性測(cè)量設(shè)備均存在誤差，得到的軌跡與地圖通常與真實(shí)情況存在著一定的偏差。如何減少噪聲帶來(lái)的影響，建立準(zhǔn)確、一致的地圖，是 SLAM 研究者們關(guān)心的主要問(wèn)題。

3.1.2 傳感器數(shù)據(jù)處理

目前，SLAM研究中使用的傳感器包括激光測(cè)距儀、單目相機(jī)、立體相機(jī)、深度相機(jī)、聲吶以及多種傳感器信息融合。SLAM的求解方式與傳感器有著重要的關(guān)聯(lián)，因?yàn)閺谋举|(zhì)上說(shuō)，SLAM所有的計(jì)算都是對(duì)傳感器數(shù)據(jù)的處理。通常來(lái)說(shuō)，傳感器的選擇不同，SLAM基本方程的參數(shù)化會(huì)有很大的差別。如基于激光傳感器的 SLAM 一般使用EKF實(shí)現(xiàn)，基于相機(jī)的則用姿態(tài)圖（Pose Graph）進(jìn)行優(yōu)化，這是由于不同傳感器的特性不同而造成的，如表1。

3.1.3 地圖描述

建圖的問(wèn)題則主要取決于地圖的類(lèi)型。不同類(lèi)型的地圖各有側(cè)重點(diǎn)，應(yīng)用于不同的場(chǎng)合。主要有以下幾種類(lèi)型。

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

2）拓?fù)涞貓D（Topological Map）。與度量地圖的精確性相比，拓?fù)涞貓D的表達(dá)角度完全不同。它采用由頂點(diǎn)和邊組成的圖（Graph）：G={V，E}的方式表達(dá)，因而更強(qiáng)調(diào)地圖元素的獨(dú)立性，以及元素之間的連通關(guān)系。其中頂點(diǎn)表示特征點(diǎn)的姿態(tài)，邊表示運(yùn)動(dòng)。拓?fù)涞貓D是一種比較緊湊的地圖表達(dá)方式。它可以放松地圖對(duì)精確位置的需要，去掉了地圖的很多細(xì)節(jié)問(wèn)題。但是，拓?fù)涞貓D很難表達(dá)具有復(fù)雜結(jié)構(gòu)的環(huán)境。拓?fù)涞貓D下一步研究的主要方向是對(duì)地圖進(jìn)行分割形成結(jié)點(diǎn)與邊的問(wèn)題。使用拓?fù)涞貓D進(jìn)行導(dǎo)航與路徑規(guī)劃也是后續(xù)需要解決的重要一環(huán)。

3）混合地圖（Hybrid Map）。由于上述地圖表達(dá)方式各有優(yōu)劣，因而有研究者認(rèn)為應(yīng)該構(gòu)建帶有層次模型的地圖，混合使用不同的表達(dá)方式來(lái)處理地圖。其核心思想是：用度量地圖表達(dá)局部結(jié)構(gòu)，用拓?fù)涞貓D表達(dá)各個(gè)小地圖之間的連通關(guān)系。此類(lèi)方法有成功的案例，但由于技術(shù)復(fù)雜，不易于推廣。

3.1.4 回環(huán)檢測(cè)

回環(huán)檢測(cè)（Loop Closure Detection），是指機(jī)器人識(shí)別是否曾經(jīng)到過(guò)某個(gè)地方。如果檢測(cè)成功，將進(jìn)行環(huán)路融合計(jì)算，找出一個(gè)最優(yōu)的更加接近真實(shí)的位置，從而顯著地減小累積誤差。

回環(huán)檢測(cè)本質(zhì)上是通過(guò)檢測(cè)觀測(cè)數(shù)據(jù)相似性來(lái)實(shí)現(xiàn)的一種算法。對(duì)于視覺(jué) SLAM，有的研究者使用傳統(tǒng)的模式識(shí)別的方法，把回環(huán)檢測(cè)建構(gòu)成一個(gè)分類(lèi)問(wèn)題，訓(xùn)練分類(lèi)器進(jìn)行分類(lèi)。但多數(shù)的SLAM使用詞袋模型（Bag-of-Words）[5]。詞袋模型通過(guò)機(jī)器學(xué)習(xí)對(duì)從周?chē)h(huán)境里大量圖像提取的視覺(jué)特征進(jìn)行聚類(lèi)，利用特征類(lèi)別建立較為精簡(jiǎn)的詞典，然后將每幅圖都用詞典中的單詞進(jìn)行表達(dá)?；丨h(huán)檢測(cè)的過(guò)程就是將當(dāng)前的圖像中的單詞與歷史圖像中的單詞進(jìn)行比較，相似度達(dá)到指定的閾值即視為回環(huán)檢測(cè)成功。

然而，錯(cuò)誤的回環(huán)檢測(cè)結(jié)果可能會(huì)使地圖變得更糟糕。該錯(cuò)誤分為兩類(lèi)：一類(lèi)是假陽(yáng)性（False Positive），也稱(chēng)作感知偏差（Perceptual Aliasing），即實(shí)際上不同的場(chǎng)景被歸為同一場(chǎng)景；另一類(lèi)是假陰性（False Negative），又稱(chēng)感知變異（Perceptual Variability），即事實(shí)上同一個(gè)場(chǎng)景被分成了兩個(gè)不同場(chǎng)景。感知偏差將使地圖質(zhì)量變差，是希望避免的。盡量多地檢測(cè)出真實(shí)回環(huán)是檢驗(yàn)回環(huán)檢測(cè)算法的重要標(biāo)準(zhǔn)。通常用準(zhǔn)確率-召回率曲線來(lái)量化一個(gè)檢測(cè)算法的好壞。

3.2 路徑規(guī)劃（Path Planning）

路徑規(guī)劃是自主移動(dòng)機(jī)器人的一個(gè)非常重要的組成部分。對(duì)于機(jī)器人來(lái)說(shuō)，只有地圖還不行，它需要知道在地圖上如何實(shí)現(xiàn)從起始位置到達(dá)目標(biāo)位置。路徑規(guī)劃就是在地圖已經(jīng)存在的前提下，按照一定的標(biāo)準(zhǔn)要求，在充滿障礙的環(huán)境中尋找從起始狀態(tài)到達(dá)目標(biāo)狀態(tài)的沒(méi)有碰撞的符合要求的路徑。

隨著不同領(lǐng)域?qū)σ苿?dòng)機(jī)器人的廣泛需求，越來(lái)越多的研究人員投入到了路徑規(guī)劃領(lǐng)域的研究，進(jìn)而提出很多有效的路徑規(guī)劃算法。比如概率路線圖法、快速搜索隨機(jī)樹(shù)法等等。

3.2.1 概率路線圖

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

與之前的算法相比，概率路線圖克服了局部極小問(wèn)題，且計(jì)算量小，適合多自由度的機(jī)器人路徑規(guī)劃。但是當(dāng)環(huán)境中有窄道時(shí)，非常有可能出現(xiàn)采樣率低的現(xiàn)象，導(dǎo)致無(wú)法找到合適的路徑。

3.2.2 快速搜索隨機(jī)樹(shù)

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

快速搜索隨機(jī)數(shù)算法仍然不夠完美，當(dāng)出現(xiàn)較多狹窄空間時(shí)，會(huì)導(dǎo)致收斂特別慢。有人采用雙向搜索技術(shù)，從起始狀態(tài)和目標(biāo)狀態(tài)同時(shí)相向地增長(zhǎng)搜索樹(shù)，從而提高搜索效率。也有人通過(guò)增加導(dǎo)向因子來(lái)引導(dǎo)節(jié)點(diǎn)擴(kuò)展方向，從而使搜索樹(shù)有目標(biāo)地生長(zhǎng)，而不是漫無(wú)目標(biāo)，這大大縮短了生長(zhǎng)時(shí)間。

3.2.3 RRT*

雖然RRT算法具有運(yùn)算速度快的優(yōu)點(diǎn)，并且能在較短時(shí)間內(nèi)找到解，但是由于RRT算法隨機(jī)性很大，已經(jīng)證明是無(wú)法找到最優(yōu)解的。為了解決這個(gè)問(wèn)題，Karaman和Frazzol在 RRT 算法基礎(chǔ)上，引入節(jié)點(diǎn)選擇機(jī)制，提出了RRT*（RRT Star）算法[8]，這種算法雖然同樣無(wú)法找到最優(yōu)解，但是可以保證找到一個(gè)接近最優(yōu)的解。

4 預(yù)期創(chuàng)新點(diǎn)

4.1 地圖創(chuàng)新點(diǎn)

基于特征的SLAM一般會(huì)因?yàn)橛?jì)算效率的問(wèn)題而采用關(guān)鍵幀的方法，因而生成的地圖一般是稀疏的點(diǎn)云地圖。在稀疏點(diǎn)云地圖上生成的占據(jù)柵格地圖，一般在復(fù)雜的環(huán)境中不足以實(shí)現(xiàn)無(wú)碰撞導(dǎo)航。如何能在保證計(jì)算效率（實(shí)時(shí)）的前提下，保證復(fù)雜環(huán)境的無(wú)碰撞導(dǎo)航是本文要實(shí)現(xiàn)的目標(biāo)之一。

4.2 路徑規(guī)劃創(chuàng)新點(diǎn)

基于RRT*路徑規(guī)劃算法獲得的導(dǎo)航路徑是一個(gè)漸進(jìn)最優(yōu)的圖，包含多個(gè)頂點(diǎn)和有向邊。每個(gè)頂點(diǎn)為無(wú)人機(jī)姿態(tài)，有向邊為控制輸入。實(shí)現(xiàn)導(dǎo)航就是讓無(wú)人機(jī)根據(jù)有向邊的控制輸入，達(dá)到每個(gè)頂點(diǎn)的姿態(tài)，最終到達(dá)目標(biāo)區(qū)域。事實(shí)RRT*給出的路徑圖并不是平滑的，是一個(gè)個(gè)離散狀態(tài)的集合，連接兩個(gè)頂點(diǎn)的邊是直線，即只對(duì)應(yīng)一個(gè)輸入，因此無(wú)人機(jī)很難無(wú)偏跟隨這條導(dǎo)航路徑。在無(wú)人機(jī)輸入極限的范圍內(nèi)，如何優(yōu)化（平滑）這條導(dǎo)航路徑，使其可以無(wú)偏跟蹤是本文要實(shí)現(xiàn)的另一個(gè)目標(biāo)。

參考文獻(xiàn)

[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.