研究目的：為了安全、準(zhǔn)確地到達(dá)指定的目的地，同時最小化其狀態(tài)估計(jì)的不確定性。

主要貢獻(xiàn)：

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

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

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

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

作者：K.Fire| 來源：3D視覺工坊

在公眾號「3D視覺工坊」后臺，回復(fù)「原論文」即可獲取論文pdf。

添加微信：CV3d007，備注：導(dǎo)航，拉你入群。文末附行業(yè)細(xì)分群。

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

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

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

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

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

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

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

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

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

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

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

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

軌跡生成函數(shù)：

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

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

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

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

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

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

作者使用Rotors模擬器搭建了仿真環(huán)境，在每個場景中，從稍微不同的起始位置啟動四旋翼機(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)勢：

• 第一個場景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ì)。

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

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

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

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

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

目前工坊已經(jīng)建立了3D視覺方向多個社群，包括SLAM、工業(yè)3D視覺、自動駕駛方向，細(xì)分群包括：[工業(yè)方向]三維點(diǎn)云、結(jié)構(gòu)光、機(jī)械臂、缺陷檢測、三維測量、TOF、相機(jī)標(biāo)定、綜合群；[SLAM方向]多傳感器融合、ORB-SLAM、激光SLAM、機(jī)器人導(dǎo)航、RTK|GPS|UWB等傳感器交流群、SLAM綜合討論群；[自動駕駛方向]深度估計(jì)、Transformer、毫米波|激光雷達(dá)|視覺攝像頭傳感器討論群、多傳感器標(biāo)定、自動駕駛綜合群等。[三維重建方向]NeRF、colmap、OpenMVS等。除了這些，還有求職、硬件選型、視覺產(chǎn)品落地等交流群。大家可以添加小助理微信: CV3d007，備注：加群+方向+學(xué)校|公司, 小助理會拉你入群。