主要貢獻：

利用前向單目相機用于狀態估計和建圖。

利用單目狀態估計和地圖系統的信息，生成一個候選軌跡庫，并從感知質量、碰撞概率和到目標的距離三個方面對每條軌跡進行評分，選擇得分最高的軌跡。

在一個實際的四旋翼機上通過仿真和實際實驗證明，與純反應式導航系統相比，文中的主動式導航方法能夠提高目標到達任務的狀態估計精度，特別是在困難場景（例如，弱紋理）下。

1 前言

微型飛行器（MAVs）既靈活又通用，適用于工業檢測、農業和貨物運輸等各種任務。為了使無人機能夠在未知環境中自主運行，需要可靠的定位和對自身位姿的估計。在用于狀態估計的不同傳感器中，攝像機重量輕且節能，非常適合于微型飛行器。

在對于基于視覺的狀態估計中，攝像機的運動對估計精度有重要影響。因此，在規劃無人機的運動時，既要考慮以上的任務，又要考慮感知質量。

本文的設計思路是一個典型的ActiveSLAM問題，作者選擇了使狀態估計精度最大化的運動，使得無人機在運行過程中不至于發生定位丟失，而且他們沒有摒棄了優化全局地圖中的運動軌跡的方法，而是用滾動的方式來解決主動SLAM問題，這樣有效地降低了計算資源。

形象說明：他們算法的示意圖如上所示，這個方法遵循圖中藍色軌跡運動，這個軌跡可以避開障礙物、到達指定的目的地并且最小化狀態估計的不確定性。相比之下，純反應式導航方案（紅色線）雖然距離更近，但會進入無紋理區域，導致狀態估計誤差大，最終無法到達指定的目的地。

2 本文架構

該系統包括單目狀態估計和建圖系統以及運動規劃系統。

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

運動規劃系統：由軌跡生成模塊和軌跡評估模塊組成。使用一種有效的軌跡生成方法，根據當前狀態估計生成候選軌跡庫。然后，我們根據感知質量、碰撞概率和到目標的距離三個指標評估每個軌跡。

3 單目狀態估計與建圖

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

首先，為了評估感知質量，他們還從SVO中提取一個活動地圖。在內部SVO維護一組稀疏點，這些點可以分為標志和種子兩類。

標志點是從不同的幀多次觀測到的三維點，它們的位置已經得到了很好的估計，對幀姿態估計的準確性貢獻最大

種子是三維點，其位置尚未精確估計。

然后從與當前幀重疊的關鍵幀中提取可見的標志，我們將這些標志記為活動地圖（圖3）。

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

4 軌跡生成

軌跡生成函數：

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

因為使用了前視攝像機，希望在前方可見的方向移動，所以通過選擇四旋翼前面一段弧上的端點來規劃軌跡，如下圖所示。這樣只需要知道圓弧的半徑r和角度θ即可，當半徑r為一個設計參數時（固定值），θ的計算公式為：

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

然后根據以下公式，選擇得分最高的軌跡Cbest驅動無人機運動：

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

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

5 實驗

5.1 仿真實驗

作者使用Rotors模擬器搭建了仿真環境，在每個場景中，從稍微不同的起始位置啟動四旋翼機，并命令它飛到指定的目的地10次，在每次運行中，一旦狀態估計偏離地面真相超過5米，就終止執行并報告試驗失敗，一旦狀態估計到給定目的地的距離小于3米，就報告試驗成功。地圖如下圖所示：

下面這些圖為實驗結果，展示了所提出方法的優勢：

第一個場景L形狀由紋理強的區域（草和石頭）和紋理較弱的區域（左下角的黑色區域）組成。可以觀察到，純反應式導航方法指令四旋翼機直接飛向目標，但軌跡經過視覺退化部分，導致狀態估計誤差較大。相比之下，文中的方法避免了四轉子進入紋理較少的區域，并且能夠保持合理的狀態估計。

第二個場景轉換在中間包含一個視覺退化區域，該區域位于起點和終點之間。雖然文中的方法能夠使無人機走了更遠的路才到達目的地，但反應式導航無法成功完成任務。

在第三場景障礙物中，地面幾乎沒有紋理，大部分視覺信息來自障礙物。在這個場景中，文中的方法和反應式導航的軌跡與前兩個場景差別不大，但是文中的方法的狀態估計誤差仍然明顯較小

第四個場景紋理完全，沒有明顯的視覺退化部分。反應式導航和文中的方法在這個場景中都表現得很好，文中的方法也有更好的性能。

?

5.2 真實世界的實驗

下圖顯示了真實世界實驗環境和實驗結果，從實驗結果可以看出，無人機并沒有直線的前往目標，而是選擇了一條距離稍長但紋理信息更豐富的路線。

6 總結

這篇文章出自蘇黎世大學，我認為這篇文章的內容、配圖以及公式推導都非常清晰明了，當時我讀這篇文章的時候還是剛接觸SLAM不就，但是看這篇文章也不會覺得困惑與無聊；作者提出的無人機ActiveSLAM解決方案也十分創新高效，通過文中的實驗結果，可以清晰地看出他們所提出方法的優勢！推薦大家閱讀！

編輯：黃飛