主要貢獻(xiàn)：

利用前向單目相機(jī)用于狀態(tài)估計(jì)和建圖。

利用單目狀態(tài)估計(jì)和地圖系統(tǒng)的信息，生成一個(gè)候選軌跡庫，并從感知質(zhì)量、碰撞概率和到目標(biāo)的距離三個(gè)方面對(duì)每條軌跡進(jìn)行評(píng)分，選擇得分最高的軌跡。

在一個(gè)實(shí)際的四旋翼機(jī)上通過仿真和實(shí)際實(shí)驗(yàn)證明，與純反應(yīng)式導(dǎo)航系統(tǒng)相比，文中的主動(dòng)式導(dǎo)航方法能夠提高目標(biāo)到達(dá)任務(wù)的狀態(tài)估計(jì)精度，特別是在困難場(chǎng)景（例如，弱紋理）下。

1 前言

微型飛行器（MAVs）既靈活又通用，適用于工業(yè)檢測(cè)、農(nóng)業(yè)和貨物運(yùn)輸?shù)雀鞣N任務(wù)。為了使無人機(jī)能夠在未知環(huán)境中自主運(yùn)行，需要可靠的定位和對(duì)自身位姿的估計(jì)。在用于狀態(tài)估計(jì)的不同傳感器中，攝像機(jī)重量輕且節(jié)能，非常適合于微型飛行器。

在對(duì)于基于視覺的狀態(tài)估計(jì)中，攝像機(jī)的運(yùn)動(dòng)對(duì)估計(jì)精度有重要影響。因此，在規(guī)劃無人機(jī)的運(yùn)動(dòng)時(shí)，既要考慮以上的任務(wù)，又要考慮感知質(zhì)量。

本文的設(shè)計(jì)思路是一個(gè)典型的ActiveSLAM問題，作者選擇了使?fàn)顟B(tài)估計(jì)精度最大化的運(yùn)動(dòng)，使得無人機(jī)在運(yùn)行過程中不至于發(fā)生定位丟失，而且他們沒有摒棄了優(yōu)化全局地圖中的運(yùn)動(dòng)軌跡的方法，而是用滾動(dòng)的方式來解決主動(dòng)SLAM問題，這樣有效地降低了計(jì)算資源。

形象說明：他們算法的示意圖如上所示，這個(gè)方法遵循圖中藍(lán)色軌跡運(yùn)動(dòng)，這個(gè)軌跡可以避開障礙物、到達(dá)指定的目的地并且最小化狀態(tài)估計(jì)的不確定性。相比之下，純反應(yīng)式導(dǎo)航方案（紅色線）雖然距離更近，但會(huì)進(jìn)入無紋理區(qū)域，導(dǎo)致狀態(tài)估計(jì)誤差大，最終無法到達(dá)指定的目的地。

2 本文架構(gòu)

該系統(tǒng)包括單目狀態(tài)估計(jì)和建圖系統(tǒng)以及運(yùn)動(dòng)規(guī)劃系統(tǒng)。

單目狀態(tài)估計(jì)和建圖系統(tǒng)：首先使用SVO來估計(jì)攝像機(jī)的6自由度姿態(tài)。利用多傳感器融合（MSF）軟件將姿態(tài)估計(jì)與IMU測(cè)量進(jìn)一步融合，得到正確的尺度和額外的速度估計(jì)。然后，將狀態(tài)估計(jì)值和圖像輸入REMODE的變體，得到正面視圖的密集三維地圖。在使用稠密地圖進(jìn)行運(yùn)動(dòng)規(guī)劃之前，利用OctoMap來降低地圖中的噪聲。

運(yùn)動(dòng)規(guī)劃系統(tǒng)：由軌跡生成模塊和軌跡評(píng)估模塊組成。使用一種有效的軌跡生成方法，根據(jù)當(dāng)前狀態(tài)估計(jì)生成候選軌跡庫。然后，我們根據(jù)感知質(zhì)量、碰撞概率和到目標(biāo)的距離三個(gè)指標(biāo)評(píng)估每個(gè)軌跡。

3 單目狀態(tài)估計(jì)與建圖

文中的單目狀態(tài)估計(jì)和建圖采用SVO+MSF進(jìn)行狀態(tài)估計(jì)，通過REMODE生成稠密點(diǎn)云進(jìn)行避障。SVO是一種非常有效的VO算法，適用于資源受限的系統(tǒng)(無人機(jī))。

首先，為了評(píng)估感知質(zhì)量，他們還從SVO中提取一個(gè)活動(dòng)地圖。在內(nèi)部SVO維護(hù)一組稀疏點(diǎn)，這些點(diǎn)可以分為標(biāo)志和種子兩類。

標(biāo)志點(diǎn)是從不同的幀多次觀測(cè)到的三維點(diǎn)，它們的位置已經(jīng)得到了很好的估計(jì)，對(duì)幀姿態(tài)估計(jì)的準(zhǔn)確性貢獻(xiàn)最大

種子是三維點(diǎn)，其位置尚未精確估計(jì)。

然后從與當(dāng)前幀重疊的關(guān)鍵幀中提取可見的標(biāo)志，我們將這些標(biāo)志記為活動(dòng)地圖（圖3）。

其次，使用Octomap來進(jìn)一步降低REMODE輸出中的噪聲。如果密集點(diǎn)云包含過多的離群值，軌跡評(píng)估模塊會(huì)錯(cuò)誤估計(jì)碰撞概率，導(dǎo)致不必要的避碰機(jī)動(dòng)。圖4展示了活動(dòng)地圖和稠密地圖，分別用MA和MD表示。

4 軌跡生成

軌跡生成函數(shù)：

p0和v0是軌跡在世界坐標(biāo)系中的初始位置和速度，來自當(dāng)前狀態(tài)估計(jì)。pf是軌跡的期望終點(diǎn)，tf是四旋翼跟隨軌跡到達(dá)pf所需的時(shí)間。輸出是函數(shù)f(t)。對(duì)于t∈[0,tf]，f(t)給出了時(shí)刻t時(shí)軌道上的狀態(tài)（位置、方向、速度）。

因?yàn)槭褂昧饲耙晹z像機(jī)，希望在前方可見的方向移動(dòng)，所以通過選擇四旋翼前面一段弧上的端點(diǎn)來規(guī)劃軌跡，如下圖所示。這樣只需要知道圓弧的半徑r和角度θ即可，當(dāng)半徑r為一個(gè)設(shè)計(jì)參數(shù)時(shí)（固定值），θ的計(jì)算公式為：

其中kθ是常數(shù)。直觀地說，θ隨著速度的增加而增加，直到達(dá)到最大值θmax。然后使用θ和l，就可以均勻地采樣弧上的端點(diǎn)。在生成N個(gè)候選軌跡后，以一個(gè)固定的時(shí)間間隔從每個(gè)軌跡上采樣J個(gè)姿態(tài)。

然后根據(jù)以下公式，選擇得分最高的軌跡Cbest驅(qū)動(dòng)無人機(jī)運(yùn)動(dòng)：

其中pcol是碰撞障礙物的概率（即密集地圖）。Rcol、Rperc和Rgoal分別給出了與碰撞風(fēng)險(xiǎn)、感知質(zhì)量和目標(biāo)進(jìn)度相關(guān)的獎(jiǎng)勵(lì)，使用D-opt標(biāo)準(zhǔn)的模擬形式來計(jì)算感知質(zhì)量，其他具體推導(dǎo)在這里不展開，感興趣可以閱讀原文。

下圖展示碰撞概率的計(jì)算。左欄顯示模擬軌跡（黑色箭頭）和障礙物（水平白帶），右欄顯示使用不同方法計(jì)算的碰撞概率。軌跡指數(shù)從左向右增加。

5 實(shí)驗(yàn)

5.1 仿真實(shí)驗(yàn)

作者使用Rotors模擬器搭建了仿真環(huán)境，在每個(gè)場(chǎng)景中，從稍微不同的起始位置啟動(dòng)四旋翼機(jī)，并命令它飛到指定的目的地10次，在每次運(yùn)行中，一旦狀態(tài)估計(jì)偏離地面真相超過5米，就終止執(zhí)行并報(bào)告試驗(yàn)失敗，一旦狀態(tài)估計(jì)到給定目的地的距離小于3米，就報(bào)告試驗(yàn)成功。地圖如下圖所示：

下面這些圖為實(shí)驗(yàn)結(jié)果，展示了所提出方法的優(yōu)勢(shì)：

第一個(gè)場(chǎng)景L形狀由紋理強(qiáng)的區(qū)域（草和石頭）和紋理較弱的區(qū)域（左下角的黑色區(qū)域）組成?？梢杂^察到，純反應(yīng)式導(dǎo)航方法指令四旋翼機(jī)直接飛向目標(biāo)，但軌跡經(jīng)過視覺退化部分，導(dǎo)致狀態(tài)估計(jì)誤差較大。相比之下，文中的方法避免了四轉(zhuǎn)子進(jìn)入紋理較少的區(qū)域，并且能夠保持合理的狀態(tài)估計(jì)。

第二個(gè)場(chǎng)景轉(zhuǎn)換在中間包含一個(gè)視覺退化區(qū)域，該區(qū)域位于起點(diǎn)和終點(diǎn)之間。雖然文中的方法能夠使無人機(jī)走了更遠(yuǎn)的路才到達(dá)目的地，但反應(yīng)式導(dǎo)航無法成功完成任務(wù)。

在第三場(chǎng)景障礙物中，地面幾乎沒有紋理，大部分視覺信息來自障礙物。在這個(gè)場(chǎng)景中，文中的方法和反應(yīng)式導(dǎo)航的軌跡與前兩個(gè)場(chǎng)景差別不大，但是文中的方法的狀態(tài)估計(jì)誤差仍然明顯較小

第四個(gè)場(chǎng)景紋理完全，沒有明顯的視覺退化部分。反應(yīng)式導(dǎo)航和文中的方法在這個(gè)場(chǎng)景中都表現(xiàn)得很好，文中的方法也有更好的性能。

?

5.2 真實(shí)世界的實(shí)驗(yàn)

下圖顯示了真實(shí)世界實(shí)驗(yàn)環(huán)境和實(shí)驗(yàn)結(jié)果，從實(shí)驗(yàn)結(jié)果可以看出，無人機(jī)并沒有直線的前往目標(biāo)，而是選擇了一條距離稍長但紋理信息更豐富的路線。

6 總結(jié)

這篇文章出自蘇黎世大學(xué)，我認(rèn)為這篇文章的內(nèi)容、配圖以及公式推導(dǎo)都非常清晰明了，當(dāng)時(shí)我讀這篇文章的時(shí)候還是剛接觸SLAM不就，但是看這篇文章也不會(huì)覺得困惑與無聊；作者提出的無人機(jī)ActiveSLAM解決方案也十分創(chuàng)新高效，通過文中的實(shí)驗(yàn)結(jié)果，可以清晰地看出他們所提出方法的優(yōu)勢(shì)！推薦大家閱讀！

編輯：黃飛