題目：單目視覺-慣性融合自動導航
摘要
自主微型飛行器（MAV）具有成本和機動性優勢，使其成為航空攝影，監視和搜索和救援等應用的理想機器人平臺。隨著平臺縮小，MAV變得更能夠在受限環境中運行，但它也引入了顯著的大小和有效載荷約束。 由于其重量輕，占地面積小，單視覺慣性導航系統（VINS）僅由慣性測量單元（IMU）和攝像機組成，在這種情況下成為最合適的傳感器套件。事實上，它是最小的傳感器套件，允許具有足夠環境意識的自主飛行。 在本文中，我們展示了使用單目VINS實現可靠的在線自主導航是可能的。我們的系統建立在配備有魚眼攝像頭，低成本IMU和異構機載計算資源的定制四旋翼測試平臺上。我們系統的主干是一個高度精確的基于優化的單目視覺慣性狀態估計器，具有在線初始化和自我外部校準。 基于GPU的基于單一密集映射模塊，基于估計的姿勢，提供廣角態態感知。最后，直接在增量式三維地圖上運行的在線軌跡規劃器可確保在雜亂的環境中進行安全導航。 提供了廣泛的實驗結果來驗證各個系統模塊以及室內和室外環境中的整體性能。
關鍵詞
空中機器人，建圖，規劃，位置估計
1.簡介
微型飛行器（MAV）是低成本，小尺寸和出色移動性的理想機器人平臺，適用于室內和室外環境中的各種應用。 由于MAV的快速動態和非視距操作的可能性，空中機器人必須能夠自主導航。
可靠的狀態估計，環境映射和避障是自主導航最重要的模塊。 全球導航衛星系統（GNSS）為狀態估計提供了一個直接的解決方案，使大量自主微型飛行器的戶外應用成功商業化。 但是，GNSS不提供任何障礙信息。 它們也不適用于需要MAV移動性優勢的狹窄，雜亂或室內環境中的操作。
在狹窄環境中操作需要小平臺。 然而，較小的平臺通常具有緊湊的尺寸，重量和功率（SWaP）限制，限制了它們攜帶有源但重的傳感器（例如雷達或激光雷達）的能力。 由于基線長度不足，小平臺的尺寸也限制了其承載立體聲或多攝像機系統的能力。 隨著平臺變小，單目視覺慣性系統（VINS）僅由低成本慣性測量單元（IMU）和攝像頭組成，由于其重量輕和占地面積小，成為唯一可行的傳感器設置。 實際上，在消費電子設備上廣泛使用的單眼VINS是最小的傳感器套件，允許具有足夠環境意識的自主飛行。
盡管具有出色的SWaP性能，但在使用單眼VINS進行自主導航時仍存在重大挑戰。使用單個攝像機無法直接觀察距離，并且要從單目VINS獲得公制測量值，需要對環境中的特征進行可靠跟蹤，以及具有足夠加速度和角速度的運動。來自IMU和相機的測量值被融合在一起以獲得狀態的度量估計。這是一個脆弱且高度非線性的過程，需要寬視場（FOV）攝像機進行特征跟蹤，精確的攝像機-IMU外部校準和良好的估計器初始化。密集3D地圖的使用對于確保檢測稀疏特征未覆蓋的障礙物是必要的。為此，通過每像素多視圖三角測量，離群值去除和深度圖融合來生成密集深度圖需要高度精確的姿態估計以及強大的計算平臺。為了關閉感知 - 動作循環，我們還需要軌跡規劃和反饋控制模塊，這些模塊在重建的密集環境表示上運行以保證安全性。
在本文中，我們提出了一個完整的系統解決方案來解決上述所有挑戰。 系統級的貢獻證明了使用最小單目VINS和完全機載處理的可行性，以通過未知混亂的環境實現安全導航。 從硬件到軟件，每個系統模塊的決策均來自仔細的工程考慮和折衷。我們選擇使用具有IMU觸發的硬件同步功能的單(?)目魚眼攝像頭來提供具有精確時間戳的廣角觀測。 我們使用一組功能強大的異構機載計算平臺來并行化計算要求較高的每像素圖像操作。
在軟件模塊層面，我們的主要貢獻在于合適實時算法的選擇，或者從我們以前的工作中得到改進。我們的算法包的主干是一個高精度，緊耦合，基于優化的單目視覺 - 慣性狀態估計器，支持魚眼相機，在線初始化和攝像機 - IMU外部校準。基于平面掃描的多視點深度估計模塊結合半全局深度平滑，為障礙物檢測提供實時密集深度圖像。使用概率截斷的帶符號距離函數（TSDF）融合將深度圖像融合到全局3D圖中。最后，基于梯度的軌跡規劃模塊直接在重建的3D地圖上運行，通過混亂的3D環境提供無碰撞軌跡。通過使用標準多旋翼控制器執行期望的軌跡來閉合感知 - 行動循環。據我們所知，我們是第一個使用這種最小傳感器套件實現此類導航功能的人。這項工作指出了一個非常小（直徑小于10厘米）的飛行器的自主方向。
本文的其余部分分為八個部分。 系統級相關工作將在第2節中簡要討論。第3節概述了我們的系統在硬件和軟件體系結構中的情況。 第4節討論我們如何使用單眼魚眼相機。 狀態估計，密集3D映射和軌跡規劃模塊分別在第5節，第6節和第7節中給出。 在線實驗結果（第8節）驗證各個模塊以及集成系統的性能。 我們在室內和室外環境中展示自主導航。 第9節總結了這項工作，并指出了未來改進的可能方向，我們的目標是將我們的框架擴展到更高速度，更低功耗，更小尺寸和更低成本的系統。
2.相關工作
在不可使用GNSS的室內和室外環境中，有一系列關于自主飛行的學術工作。 當我們考慮對整個系統有貢獻的單個模塊時，該集合變得更加廣泛。 在本節中，我們僅回顧使用低成本視覺傳感模式的最相關的系統級工作，并將關于各個模塊的討論留給相應的部分。
基于立體視覺的方法已被廣泛研究用于航空導航。參考文獻1，使用三個板載計算單元，FPGA用于實時立體匹配，Gumstix用于實時嵌入式處理，Intel Core2Duo用于視覺測距，映射和規劃。通過使用由Octomap library2提供的概率體素圖，路徑規劃器被設計為提供到指定航路點的無障礙路徑。松散耦合的視覺 - 慣性擴展卡爾曼濾波器（EKF）用于狀態估計。然而，由于立體深度估計的空間基線要求，很難使平臺小型化。同樣，參考文獻3給出了另一個基于視覺的映射和規劃框架，其映射模塊在其外部運行。還使用板外處理來實現循環閉合模塊。然而，除了幾個前視立體相機之外，另一個向下看的相機用于基于光流的狀態估計。然而，在我們的工作中，我們展示了如何只有一臺攝像機足以實現所有導航功能。
已經提出了使用各種傳感器放置和計算基礎結構的基于單目視覺的方法。結合SVO4和REMODE5，設計了一種重量輕，成本低的飛行平臺，可以自動飛行并實時同時進行密集的三維重建。該平臺配備有向下攝像頭，用于狀態估計和映射，這不足以在雜亂環境中進行障礙物檢測。此外，參考文獻6將映射模塊卸載到地面站，這會導致高延遲到不適合在線導航的點。通過僅使用包含足夠傳感器的智能手機，包括IMU和單目相機，參考文獻7在GPS拒絕環境中實現自主飛行。但是，在這項工作中沒有開發出映射或避障模塊。與我們的系統配置最相似的是參考文獻8，其中前視單目視覺系統用于閉環自主導航。然而，使用半稠密方法的深度估計模塊仍然是非車載的9。
在參考文獻10中，作者提出了具有多個傳感器的雙層融合框架，旨在在諸如船載環境的挑戰性環境中開發魯棒的自主導航系統。 使用冗余傳感器套件，參考文獻10主要致力于提供安全路徑，并在極端環境中提供令人信服的強大導航實驗。 然而，出于不同的目標，我們考慮建立一個具有最小傳感的敏捷飛行平臺。
在固定翼飛行器上也可以實現視覺導航。 基于IMU和平面激光，參考文獻11提出了一種高斯粒子濾波器的新穎擴展來解決定位問題。 通過展示具有挑戰性的室內環境實驗，作者對已知室內環境中的高速飛行進行了深思熟慮。
另一個研究方向涉及使用基于學習的技術進行end-to-end的導航動作生成。 代表作品包括參考文獻12。 盡管是一個很有希望的方向，但基于學習的方法尚未達到復雜未知環境中安全飛行的成熟水平。
3. 系統概述
我們現在介紹我們的四旋翼實驗測試臺的硬件設計和軟件架構。 我們滿足傳感器選擇，傳感器同步，計算能力分配，監控，調試工具，實驗接口和模塊化系統設計的要求。 我們將解釋每個系統設計決策背后的基本原理以及系統模塊的描述。
3.1 硬件結構
我們的目標是設計一個模塊化，緊湊，耐用，易于構建的機器人測試臺。 我們的機載系統由兩個主要模塊組成：感知核心和四旋翼機架。 感知核心集成了車載電腦和傳感器，四旋翼機架由機械結構，電機，螺旋槳和電池組成。
3.1.1 感知核心
感知核心是一個可拆卸的模塊，可容納所有計算單元和傳感模式。 它可以獨立測試狀態估計和映射模塊，并可與其他機械框架一起使用。
我們使用兩臺電腦組成強大的異構計算平臺（圖2）。 其中一款采用雙核英特爾i7-5500U處理器，運行頻率高達3.00 GHz，配備8 GB內存和256 GB固態硬盤，功耗約為15W。 另一款車載計算機是NVIDIA TX1，它由載板上的核心組裝而成。 TX1上的256個NVIDIA CUDA GPU內核使其特別適用于并行計算深度圖像和TSDF融合。 TX1配備了四核ARM Cortex-A57處理器和4 GB內存，功耗約為15 W. 有關感知內核中每個組件的更多詳細信息可以在表1中找到。兩個機載計算機使用以太網電纜連接。 兩臺計算機之間的通信通過使用ROS基礎設施完成。（計算單元的成本好高，這東西我畢設不一定能用）
我們的板載計算資源涵蓋了大多數廣泛使用的開發平臺：x86，ARM和GPU，使其能夠高度靈活地開發高級算法。 它顯著縮短了開發周期，因為研究人員不需要在代碼級優化上花費過多的時間。 然而，我們強調，如果通過數字比較，我們的機載資源只與Qualcomm SnapDragon 835等先進的手機處理器相媲美。這意味著使用感知核心開發的算法可以轉移到低功耗移動設備 計算平臺經過充分的代碼級優化。
機載攝像頭和IMU也包含在感知核心中。我們使用具有752×480分辨率的前視矩陣視頻藍光CTX-MLC200w全局快門攝像頭。 它配備了220度超廣角魚眼鏡頭。 DJI A3飛行控制器同時用作IMU和姿態穩定控制器。由于在雜亂的室內環境中缺乏可靠性，我們禁用了A3中的GPS和磁力儀功能。
高性能的視頻融合需要準確的時間戳。 在我們的系統中，這是通過硬件同步實現的，其中個人圖像捕捉由DJI A3飛行控制器的同步信號觸發。 更具體地說，我們在相機的硬件實時控制器中使用觸發模型。 DJI Onboard SDK提供了一個界面，用于生成一束脈沖信號和相應的IMU測量值。 請注意，IMU以比攝像機更高的頻率運行，并且如果IMU測量對應于圖像，則會標記IMU測量。 只需一根附加電纜即可構成硬件同步設置。
3.1.2 四旋翼結構
四旋翼機架由機械結構，電機，螺旋槳和電源模塊組成。 框架采用碳纖維管和鋁制部件制造，輕巧耐用。 整個系統如圖1所示。總重量，包括感知核心，是1.8公斤。 Tip-to-tip距離為33 cm。我們使用DJI E310電機和DJI Takyon Z425-M ESC作為升降系統。 我們使用DJI Phantom 4 Pro智能電池提供約15分鐘的飛行時間。 DJI Lightbridge 2用于在iPad上實現遠程桌面功能。
3.2 軟件結構
我們系統的軟件架構如圖3所示。利用ROS作為通信中間件，我們在兩臺板載計算機之間分配計算負載，其原理是在NVIDIA TX1上執行密集像素操作。 兩個車載電腦使用以太網連接。
3.2.1 算法流水線
在Mini i7計算機上，100 Hz IMU測量以及20 Hz同步魚眼圖像融合在視覺慣性狀態估計器（第5節）中，以獲得密集映射，軌跡規劃和姿態的姿態，速度和姿態，以及 反饋控制。 估算器輸出為100 Hz，這足以實現靈活的空中機器人的反饋控制。 為了利用最初針對針孔圖像設計的運動立體算法，我們使用感興趣區域（ROI）提取模塊（第4.2節）將魚眼圖像裁剪成兩個針孔圖像以覆蓋180度水平FOV。 然后將針孔圖像與估計的姿勢捆綁在一起并發送到TX1以進行深度估計。
（分成兩塊：1.i7-提取roi，估計狀態
2.tx1-深度估計）
利用NVIDIA TX1的GPU主板，使用具有半全局平滑的密集多視圖運動立體聲以10Hz計算深度圖像（第6節）。 使用GPU加速截斷符號距離函數（TSDF）融合將深度圖像轉換為全局地圖，也以10 Hz運行。
全局地圖被發送回Mini i7計算機以輔助軌跡規劃（第7節），其中生成引導機器人朝向用戶指定目標的無碰撞時間參數化軌跡。 使用DJI A3飛行控制器中的姿態和推力控制接口執行軌跡。
與許多依賴向下攝像頭進行狀態估計的現有系統不同，我們的系統僅使用一個攝像頭來滿足所有傳感需求。 我們通過在線室內和室外實驗（第8節）展示我們的最小傳感設置足以實現完全自主導航。
3.2.2 遠程監視和調試接口
除了機載軟件，我們還利用DJI Lightbridge 2實現了一個iOS應用程序，用于遠程監控和調試。 Lightbridge直接將Mini i7計算機的HDMI輸出流式傳輸到iPad Pro。 這使我們即使在機器人飛行時也可以實時查看桌面。 我們使用Lightbridge中的數據透明傳輸協議將虛擬鍵盤和鼠標輸入從iPad傳輸到機載計算機。 通過這種方式，我們可以直接使用iOS應用程序在機載計算機上運行可視化工具，如rviz。 它甚至使我們能夠在四旋翼飛行器的同時調試和重新編譯機載代碼。 此設置獨立于任何外部基礎設施，這使其非常便于現場測試。
4. 魚眼相機
魚眼攝像機提供的超寬視野是圍繞障礙物安全導航的關鍵，但它還伴隨著大量的鏡頭失真，必須對其進行建模和校正。 在本節中，我們將討論如何建模魚眼，以及必要的預處理步驟，以便能夠使用魚眼圖像進行視覺慣性狀態估計和密集深度估計。
4.1 魚眼相機模型
我們使用參考文獻14,15中提出的統一球體模型模擬魚眼相機。 如圖4(a)所示，相機投影模型是一個函數πc：R3→Ω，這個函數模擬了3d點及其在2d圖像平面上的位置之間的關系。反投影模型πc-1：Ω→R3是它的逆。在該模型中，圖像中的3D點pc∈R3首先被投影為單位球面上的點pc'，然后變換成2D圖像平面上的uc。鏡頭失真在πc(?)中處理。 這可以在數學上描述為
反投影模型是pc'=πc-1(uc)，可以將2D圖像平面上的點映射回單位球。
為了在單眼視覺慣性狀態估計器（第5節）中利用魚眼攝像機的覆蓋感知范圍，我們直接在單位球面上定義圖像測量殘差。 更多細節將在第5.5.3節中給出。
4.2 ROI提取
多視角稠密深度估計的過程可以通過使用一維掃描線搜索加速。 然而，存在顯著的透鏡畸變使得難以直接將這些加速技術應用于魚眼圖像。 我們注意到，對于主要經歷水平運動的空中機器人而言，大的水平FOV是確保安全的最關鍵方式。為此，我們引入了一個感興趣區域（ROI）提取模塊，將魚眼圖像轉換為兩個無失真的針孔圖像，覆蓋180度水平視場。
我們考慮使用虛擬理想針孔相機來捕捉無失真圖像。 根據應用需求手動選擇虛擬攝像機的FOV，從中可以唯一確定虛擬攝像機的投影函數πv(·)。 虛擬攝像機相對于魚眼攝像機圖象(Rvc)的方向也由用戶選擇，如圖4(b)所示。 對于虛擬圖像中的像素uv，我們有
一旦我們在魚眼圖像中獲得相應的像素位置，我們使用子像素插值來確定強度值。
我們在系統中使用的兩個ROI的圖示如圖4(b)所示，其中相應的魚眼視圖標記在圖4(c)中。 從ROI獲得的兩個無失真針孔圖像如圖4(d)所示。 兩個針孔圖像與物理光軸成±45度。雖然理論上我們只需要將每個針孔圖像的水平FOV設置為90度，但我們將兩個虛擬圖像的FOV設置為實際實現的120度。 為了給中心區域提供冗余。
5. 單目視覺慣性狀態估計
狀態估計是自主飛行的核心。 建圖和路徑規劃需要高精度的度量狀態（位置，方向和速度）。 在本節中，我們提出了一種基于單目緊耦合滑動窗口的視覺慣性狀態估計器，它為整個系統提供準確的狀態。 由于單目視覺慣性融合是一種脆弱且高度非線性的過程，因此需要穩定的特征跟蹤和良好的估計器初始化。 在下文中，我們將詳細介紹測量預處理，穩健估計器初始化過程以及緊耦合優化框架。 這是我們以前的工作16,17的延續。
5.1 相關工作
最近，我們已經看到了很多關于視覺慣性估計的工作，無論是單目攝像機18-20，立體攝像機21還是RGB-D攝像機22.基于框架，我們可以將VINS分為兩類：一類是 基于過濾的算法18-20,23-26，而另一個是基于圖優化或基于束調整的方法16,21,27.基于過濾的算法通常被認為在計算上更有效。 主要缺點是早期固定線性化點可能導致次優結果。通過重復線性化過去狀態，基于圖優化的方法可以實現更好的性能，同時它們需要更多的計算資源。 從數學上講，這些方法是相同的，但是以不同的形式實現。
對于單目視覺慣性融合，需要對系統進行精確初始化，但單目攝像機不能直接提供度量測量。我們早期的工作16,17,28提出了一種基于優化的線性估計器初始化方法。然而，由于不能在原始投影公式中對傳感器噪聲進行建模，因此在特征深度分布在整個范圍（例如，室外環境）中的環境中失敗。此外，我們早期的工作并未考慮陀螺儀偏差。 后來參考文獻29中引入了另一種封閉形式的解決方案。 在參考文獻30中提出了對這種封閉形式解決方案的修訂。 這種封閉形式的解決方案對噪聲傳感器數據很敏感，因為作者無法在不同的持續時間內為不同的慣性積分建模測量噪聲。參考文獻31中，提出了一種基于SVO32的重新初始化和故障恢復算法。在這種方法中，首先使用慣性測量來穩定MAV的姿態，然后啟動SVO以穩定位置。 MSF26松散地融合了視覺和慣性信息。
處理IMU測量的一種有效技術是預積分，其通過相對運動約束的重新參數化避免重復積分。 該算法首先在參考文獻33中提出。 在我們之前的工作16中提出了考慮流形不確定性傳播的方法。在參考文獻34中提出了進一步改進IMU偏差并將它們與完整的SLAM框架相結合的建議。
在本文中，為了實現準確的實時估計，我們使用基于緊密耦合優化的單目視覺慣性框架和強大的初始化。 我們的VINS是一個稀疏且基于特征的框架，它提取和跟蹤特征，最大限度地減少了后端的視覺和慣性幾何誤差。
5.2 滑動窗口公式
我們在固定的時間間隔內進行IMU和相機測量以進行狀態估計。 由于使用多視圖約束，這提供了良好的準確性。 它也享有恒定的時間復雜性。 如圖5所示，固定窗口中包含多個IMU和攝像機測量。 滑動窗口中的完整狀態向量定義為（為簡化演示而忽略轉置）
其中xk是第k幀的狀態，它包含世界坐標系下的位置、速度、旋轉以及body坐標系下的陀螺儀偏轉、加速度偏轉。
在這里，我們將IMU坐標系視為body坐標系。 世界坐標系與重力垂直的有關。 在初始化過程中解決重力矢量后，將設置世界坐標系。 在整個估算框架中，我們使用四元數來表示外部參數旋轉xcb，其包含從相機到IMU的旋轉和平移。 n是滑動窗口中關鍵幀的數量，m是滑動窗口中的特征點數量，λi是第i個特征點在單位球面上的第一次觀察的反向深度。
5.2 測量預處理
我們的狀態估計框架中存在兩種測量。 一個是圖像，另一個是IMU測量。 在將這兩種測量結合到估計器之前進行預處理。 我們在預處理步驟中跟蹤和檢測圖像中的特征。 同時，IMU測量是預先整合的的。 請注意，IMU測量不僅受噪聲影響，還受偏差影響。 因此，我們特別考慮IMU預積分和以下優化過程中的偏差。
5.3.1 特征處理前端
對于每個新圖像，現有特征由KLT稀疏光流算法35跟蹤。 同時，檢測到新角點特征以保持每個圖像中最少特征（100-300）。 通過在兩個相鄰要素之間設置最小20-30像素的間隔，檢測器強制實現特征的均勻分布。 在進入外點拒絕之前，使用魚眼攝像機模型將特征投影到單位球體。 外點拒絕只是由RANSAC執行。
在此步驟中也使用了關鍵幀(Keyframes)。 我們有兩個關鍵幀選擇標準：其中一個是平均視差——如果跟蹤要素的平均視差超出某個閾值，我們會將此圖像視為關鍵幀( 請注意，不僅平移而且旋轉都會導致視差; 但是，在僅旋轉運動中不能對特征進行三角測量。 為了避免這種情況，我們在計算視差時使用IMU傳播結果來補償旋轉。)； 另一個標準是跟蹤質量， 如果跟蹤特征的數量低于某個閾值，我們也會將新幀視為關鍵幀。
5.3.2 IMU預積分
給定兩個時刻k和k + 1對應于圖像第bk，bk+1幀，我們可以預積分局部幀bk中的線性加速度和角速度
在上述公式中，⊕代表四元數乘法操作符。可以看出，預積分部分可以僅用[k，k+1]內的IMU測量獲得，因為它與狀態(位置和速度)無關。
5.4 初始化
由于緊耦合視覺 - 慣性優化是非線性的，我們需要一個良好的初始猜測來引導整個系統。我們不能直接使用IMU預積分結果作為初始猜測，因為我們不知道初始加速度和速度。 此外，IMU測量受到未知偏差的影響，尤其是陀螺儀偏差，這將極大地影響估計。
我們采用松耦合傳感器融合方法來獲得初始值。 我們發現僅視覺SLAM或Motion from Motion（SfM）具有良好的初始化特性。 在大多數情況下，僅視覺系統可以通過相對運動方法的派生初始猜測來自我引導，例如八點法36，五點法37，齊次和基本矩陣。 通過僅視覺SfM和IMU度量預積分信息之間的松散耦合對齊，我們可以粗略地恢復尺度，重力，速度和陀螺儀偏差。 這足以引導非線性系統。
首先，我們在滑動窗口中獲得成比例位置和特征點位置。其次，我們將視覺和IMU信息對齊以校準IMU陀螺儀偏差，尺度，速度和重力矢量。 雖然第一步是非線性的，但它可以自我引導并且非常穩定。 第二步是線性形式。
在初始步驟中需要相機和IMU之間的外部參數xcb。 注意，我們這里只需要對外部參數(安裝關系)進行初步猜測，并且將在非線性優化中進行準確的估計。
5.4.1 Up-to-scale visual SfM in sliding window
在此步驟中，我們嘗試在窗口內構建僅視覺結構（按比例縮放相機位置和特征點位置，可以理解為歸一化坐標）。
我們首先在滑動窗口中選擇包含足夠滿足視差要求的特征點數的兩個關鍵幀。 接下來，我們使用五點法37來恢復這兩幀之間的相對旋轉和按比例的平移，然后我們確定平移的尺度并對這兩幀中觀察到的特征進行三角測量。 基于這些三角測量特征，執行Perspective-n-Point（PnP）方法以估計滑動窗口中的所有幀位置。
然后應用全局完整的Bundle Adjustment38以最小化所有幀中重投影誤差。
公式中的s為未知的尺度，將在后文討論。
5.4.2 視覺-慣性對齊
IMU偏差初始化：
假設在現在的窗口中IMU陀螺儀的偏差bg。考慮窗口中的兩個連續幀bk，bk+1，我們從視覺框架中得到兩個相關的旋轉qbkb0，qbk+1b0，以及從IMU預積分得到的旋轉傳播結果γbk+1bk。我們通過最小化這兩個之間的誤差來估計陀螺儀偏差(最小化視覺求出的旋轉與陀螺預積分出來的結果間誤差)：
其中B代表窗口中的IMU測量值。 通過求解這個最小二乘問題，我們可以得到bg的估計。 然后我們就利用bg更新αbk+1bk，βbk+1bk。至于加速度計偏差，在初始化過程中很難解決，因為當我們同時求解gb0時，需要足夠的旋轉運動來區分加速度計偏差和重力。 然而，給出粗略的初始猜測是足夠的，因為我們將在初始化之后不斷改進偏差。 因此，我們在初始化步驟中將加速度計偏差ba視為零。
速度，重力矢量和度量標準初始化（不太懂，得看一下IMU初始化）
我們定義了我們想要估計的變量
其中s是尺度參數，它將視覺與IMU測量隱式提供的實際度量標度對齊。 基于牛頓運動學，我們可以得到以下形式的等式：
注意其中的Δtk的意義是兩個連續關鍵幀之間的時間間隔。 通過解決最小二乘問題(10)我們可以得到每個local坐標系中的速度，以及視覺坐標系中的重力矢量(·)b0，還有尺度參數。來自視覺的平移分量pbi將被縮放到單位度量。我們將所有變量從坐標系(·)b0旋轉到世界坐標系(·)w，其中重力矢量是垂直的。 此時，初始化過程完成，這些度量值將被饋送到緊耦合非線性視覺慣性估計器。
5.5 視覺慣性定位緊密耦合
在狀態初始化之后，我們繼續使用滑動窗口非線性估計器來進行高精度狀態估計。 通過在非線性優化框架中包含IMU偏差校準，這是我們早期工作16,17的擴展。
5.5.1 公式
我們最小化所有測量殘差的Mahalanobis范數的總和，以獲得最大的后驗估計a：
5.5.2 IMU測量模型
根據運動學理論，預積分IMU的測量殘差可以定義為
其中[·]xyz提取四元數q的向量部分，是error-state的近似值。
5.5.3 相機測量模型
為了利用大型FOV攝像機的優勢，我們在參考文獻中39提出了與魚眼相機型號相匹配的在單位球面上定義攝像機測量殘差。 如圖6所示，第j個圖像中的第l個特征的相機殘差被定義為
5.5.4 邊緣化(Marginalization)，理解為舍棄?
為了限制基于圖優化的方法的計算復雜性，結合了邊緣化。 我們選擇性地將IMU狀態xk邊緣化并從滑動窗口中獲取λl，同時將對應于邊緣化狀態的測量值轉換為先驗狀態。
在我們以前的工作16,17我們使用了雙向邊緣化有選擇地刪除基于場景的視差測試最近的或舊的狀態。 如果是關鍵幀，則將新幀添加到滑動窗口，并且最舊的幀狀態通過其相應?(corresponding)的測量被邊緣化。 否則，如果出現非關鍵幀，我們會將第二個最新幀邊緣化。 這種邊緣化方案可以在窗口中保持空間關鍵幀，同時限制預積分的IMU測量的不確定性。
然而，我們之前策略的一個缺點是邊緣化第二個最新幀會產生密集的先驗，這將破壞系統的稀疏性，增加計算復雜性。為了避免這種情況，我們稍微修改了邊緣化第二個最新框架的策略。 當非關鍵幀到來時，我們丟棄視覺測量并保持IMU測量值對應于窗口中的第二個最新幀，而不是邊緣化所有測量值。盡管我們放棄了一些視覺測量，但這些測量并不重要，因為它們對應于小范圍運動，這不影響視覺。一個潛在的缺點是我們延長了預積分的時間間隔，這將導致滑動窗口中的協方差變大。 但是，這種策略可以保持系統的稀疏性，使其在計算受限的平臺上高效工作。
我們基于邊緣化來構建新的先驗。 邊緣化使用Schur補集進行。 直觀地，通過邊緣化，保留了被移除狀態的重要信息，并且計算復雜性受到限制。
5.6 用于反饋控制的IMU傳播(?propagation)
請注意，IMU測量的速度遠遠高于視覺測量。 我們的非線性優化估計器的頻率受到視覺測量的限制。 為了有利于實時控制的性能，估計器的輸出直接傳播最新的IMU測量值，這些測量值用作控制環路中的高頻反饋。(翻譯有問題，To benefit the performance for real-time control, the outputs of the estimator are directly propagated with the newest IMU measurements, which serves as the high-frequency feedback in the control loop.)
6. 單目稠密地圖
作為自主四旋翼導航的核心，建圖模塊提供了重建周圍環境的感知能力。 這種重建是圍繞障礙物安全有效導航的支柱。 本工作中使用的方法是繼續我們的初步結果40。我們使用來自單目的多個圖像的時間成本聚合( ?Temporal cost aggregation)來消除基線限制。 這之后是為無紋理環境去除異常值和深度傳播進行半整體平滑。 我們利用NVIDIA TX1上的GPU，并使用CUDA實現我們的算法。 使用截斷的有符號距離場(TSDF)融合局部深度圖像，以提供直接用于軌跡規劃的全局地圖。
6.1 相關工作(RGB-D，雙目缺點，半稠密建圖缺點)
在稠密建圖領域存在廣泛的學術工作，并且使用各種各樣的傳感器，例如RGB-D相機，立體相機和單目相機。 在RGB-D相機設置中，KinectFusion41同時解決了定位和建圖問題。 它使用迭代最近點（ICP）來解決相機姿態，然后通過TSDF融合重建全局地圖。 參考文獻42,43中提出了使用RGB-D傳感器的自主飛行。 利用由Kinect構造的已知網格圖，作者使用直接半模跟蹤進行姿態估計。 然而，RGB-D傳感器有其自身的局限性，嚴重限制了它們在室外環境中的使用。(受光照影響嚴重，因為深度是由結構光取得的)。
空間立體視覺相機是另一種流行的傳感器配置。 參考文獻44中提出了半全局匹配立體方法。通過結合像素成本(?pixelwise cost)和平滑約束，作者提出了一種全局能量函數來求解視差圖并通過動態規劃優化其近似版本。 如參考文獻45中所提出的，可以使用基于機器學習的方法來進一步改善立體匹配成本。空間立體聲很容易受到校準問題的影響，因為它取決于已知的相機外部參數。 此外，基于立體視覺的方法的基線要求從根本上限制了其對小規模平臺的能力。
對于單目，參考文獻46提供了一種自校準方法，可以在束調整框架中共同解決相機的內在3D標志點以及相機姿勢。 作者通過新穎的匹配成本定義在小運動條件下生成精細深度圖像，該定義通過一組圖像獲取候選深度之間的強度和梯度差異。 該算法顯示出令人印象深刻的深度圖像，但每次深度圖像估計的時間消耗（10分鐘）對于實時四旋翼應用是不可接受的。
DTAM47結合了數據項和正則化項，形成全局能量函數。 通過使用原始對偶(?primal-dual)方法，DTAM通過total variation有效地優化了問題。 類似的工作REMODE5，也使用原始對偶方法，并通過總變差優化問題結合了貝葉斯估計。 但是，它們都需要桌面級(desktop-grade)GPU才能實時使用。 半稠密建圖(Semidense mapping)9是另一項研究。 在基于特征的稀疏映射和基于像素的密集映射之間，半稠密方法選擇具有高梯度的像素，這使得整個框架在CPU上實時運行。 然而，這種像素密度不足以進行安全導航。
6.2 深度估計
如4.2節中所討論和圖4（b）所示，從魚眼圖像中提取兩個無失真虛擬針孔圖像，以提供180度水平FOV。 這兩個圖像在完全相同的算法管道中處理建立稠密地圖。 為簡潔起見，我們僅為其中一個提供算法流程。
我們的深度估計是根據關鍵幀進行的。 對于每個關鍵幀Ir，多個測量幀Im用于深度更新。任何時候只存在一個關鍵幀，并使用距離度量來切換關鍵幀。 如果該幀與當前關鍵幀之間的姿勢轉換大于距離閾值ζ，則幀被視為新的關鍵幀(圖7)
受平面掃描算法48,49的啟發，我們為關鍵幀中的每個像素采樣不同深度的多個候選虛擬平面。 這些平面垂直于規范視點的z軸。 關鍵幀中的每個像素首先投影到虛擬平面，然后反投影到測量(measurement frame)幀。 通過測量強度差異，我們獲得每個深度中每個像素的相似性成本(similarity cost)。 我們收集所有相似性并把它們放在一個3D量(?volume)里。
當存在多個測量圖像時，我們可以獲得每個測量圖像的相似成本量。 由于所有成本量都以參考圖像Ir為條件，因此我們可能會將所有成本量匯總到一個成本量中，以降低其對噪聲和異常值的敏感度。 使用GPU并行實現可以加速此過程。 直觀地，對于每個像素，其實際深度是對應于聚合成本量中的最小相似性成本的深度。
然后應用半全局平滑44來進行無紋理區域的異常值去除和深度插值。 拋物線擬合用作深度細化的最后一步。
6.2.1 關鍵幀選擇(Keyframe selection)
設置距離閾值的動機是確保我們選擇的每個關鍵幀都有足夠的信息來提供可靠的深度圖像。
在我們之前的工作40中，關鍵幀由VINS框架自動選擇。 但是，存在兩個限制：
a. 正如我們在5.3.1節中提到的，VINS中的視差檢查是選擇具有足夠視差的關鍵幀。 但是，不能單獨配置此關鍵幀標準以滿足稠密地圖的要求。
b. 為了識別附近的深度，掃描平面被預先參數化，這意味著深度感知的范圍是固定的。 但我們檢查VINS中的平均視差，計算遠離相機的特征，并減少平均視差。 考慮到大規模環境的情況，關鍵幀的度量距離將變得非常大，使得由于近深度像素的溢出而觀察不到附近的障礙物。
作為改進，在這項工作中，我們使用兩個閾值來控制關鍵幀選擇。 當我們沒有足夠的深度感知信息時，第一個閾值ι用于對抗懸停。 如果平移距離小于第一個閾值，我們將只進行聚合步驟。第二個閾值ζ針對關鍵幀選擇。 如前所述，此關鍵幀閾值為適應最近的采樣平面配置。 根據經驗，我們將此閾值處理得足夠大，以生成具有足夠深度提取信息的關鍵幀，但又小到足以避免遮擋和近平面的深度不可觀察性(足夠小不可以理解)。
6.2.2 GPU加速的時間成本聚合(GPU-accelerated temporal cost aggregation)
我們采用反向深度λk對平面L進行采樣，其中k屬于[1,L-1]。第K個深度平面(enumerated plane)的深度是dk
絕對差之和(SAD)用于計算相似性成本。 應用固定尺寸的3×3窗口。 因此，成本可以表示為
通過首先將關鍵幀中的像素u'反投影到由候選平面定義的3D點，然后將該3D點重新投影到測量幀上來找到測量幀中的像素位置。 對于關鍵幀中的每個像素u，我們對其相鄰像素重復采用該過程來計算相似性成本，如公式(16)所示。 通過聚合來自在不同時間捕獲的多個測量圖像的相似性成本來計算成本量ESAD。為了增加浮點像素位置um的光強精度，使用雙線性插值。 這將在使用紋理存儲器(texture memory)的GPU計算期間自動發生，其不消耗任何額外的計算資源。
6.2.3 半全局優化
時間成本聚合(Temporal cost aggregation)旨在通過使用多個測量來降低圖像噪聲的靈敏度。 但是，簡單的winnertakes-all策略都不會得到可靠的結果，因為重復圖案很容易發生錯誤。 此外，在無紋理區域中，cost volume將包含不同深度之間的類似cost，這導致深度估計的模糊性。 為此，我們的深度估計器采用了基于平滑的優化方法。
由于我們使用枚舉平面(?enumerated planes)來表示深度圖像，因此深度優化問題現在變成平面索引優化問題。 將K表示為我們嘗試優化的平面索引地圖。 將像素成本(?pixelwise cost)和平滑約束組合的全局能量函數E(K)表示為
其中Ω是關鍵幀的圖像域，Ku是一個在像素u索引下的enumerated plane(可能是u對應深度的同深度圖像)。T[·]為指標函數，若內部為真則為1，否則為0。每個像素的能量函數包括三種項：一個直接從成本量(cost volume)中提取的數據項和兩個正則化項。第一正則化項P1對所列舉的深度差僅為1的相鄰像素進行能量懲罰。第二正則化項P2對較大的深度差給出較高的懲罰。
由于全局最小化是一個無法在多項式時間(?polynomial time)內求解的NP完全問題，因此在參考文獻44中提出了半全局匹配（SGM）。在這項工作中，我們使用四路SGM，它在速度和精度之間進行了很好的權衡。
因為最后一項minjLr(u-r,j)小于前兩項之和，因此它用于減小數值，當ur不變時它也不變，這保證了Lr(u,K)不受影響。因此，該問題被簡化為沿r方向的一維問題。 通過使用GPU可以并行求解近似成本函數。 偽代碼在算法1中示出，并且SGM計算的直觀說明在圖8中示出。
6.2.4 深度圖像的后處理
在半全局優化之后，可以使用簡單的winner-takes-all策略通過選擇每個像素的最低成本S(k)來獲得深度。 然而，由于深度是在離散設置下采樣的，我們使用拋物線插值來獲得更好的深度，這是兩個深度樣本之間的極值點，
其中a，b，c是拋物線的系數，Ku是平面索引(像素u對應的平面)，其在像素u處達到最低成本，Ku*是極值點處的插值平面索引(index)。 因此，可以通過du計算像素u的估計深度。
這里有兩個標準用于消除異常值。 首先，對于具有一些逆深度采樣的像素，我們無法計算相似性成本，因為在該反深度處的測量幀中投影的相應像素是未知的。對于那些像素u，我們在相應的反深度的3D cost volume 中將其標記為未定義，并且稍后在SGM步驟中將Lr設置為零以消除缺失成本的影響。 最后，輸出深度圖像處的那些像素被設置為未定義。 其次，如果它不是擬合拋物線的絕對最小值，我們會降低深度。 具體而言，必須忽略位于第0或第(L-1)平面的最小成本; 在這種情況下，像素的深度將設置為未定義。 此外，如果兩個相鄰平面的成本之和小于最小成本的兩倍，則相應的像素也將被設置為未定義。（該部分將深度圖像中的一些異常像素設置為未定義）
6.3 全局稠密地圖
我們用uncertainty-aware truncated signed distance field(TSDF)法融合所有相機不同位置對應的深度圖。用開源CHISEL TSDF50實現，6.3.2節改進了不確定感知深度融合(uncertainty-aware depth fusion)，6.3.3節改進了算法并行化。
6.3.1 TSDF
整個環境可以建模成一個三維立體符號距離場。 帶符號距離函數Φ(x)：?3→?是體素x到其最近曲面的距離，如果體素在曲面和相機之間則為正，否則為負。 由于我們只對重建曲面感興趣，因此我們使用截斷的帶符號距離字段：
為了獲得更好的內存效率，CHISEL使用兩級混合數據結構。 第一級塊是空間散列映射，由Nv3體素的固定網格組成。 從增長的堆動態分配塊并基于其整數坐標在空間3D哈希映射中索引。 第二級是尺寸為L×L×L的3D立方體體素，其中L是度量中的映射分辨率。 草圖如圖9所示。
CHISEL提供了兩種深度掃描融合方案。 由于我們在深度圖像估計中使用虛擬針孔相機，因此基于camera frustum的投影映射比光線投射更有效。 camera frustum是一個軸對齊的邊界框，它本質上取決于相機，我們傾向于融合最近的平面和最遠的平面。 我們首先檢查與camera frustum相交的每個塊，如果是第一次檢查，我們分配塊。 然后，將在更新過程中枚舉塊中的每個體素。(這一段不太能看懂)
在這種投影建圖方法中，每個體素將被投影到相機平面上并與來自深度圖像的深度值進行比較。 我們將差異表示為ex∈?+; 只有當差異在截斷距離[-τ，τ]內的，體素才會被更新，否則它將被忽略。 體素更新算法在算法2中詳述。
6.3.2 不確定性融合(?Uncertainty-aware fusion)
由于遠程視覺深度估計通常伴隨著更大的深度不確定性，我們通過使用動態截斷距離Ψ(d)來編碼不確定性。 將σλ定義為逆深度λ中的深度測量的標準偏差。 然后可以使用線性化來近似深度測量的標準偏差d：
其中σλ設定為λmin，這是逆深度場中的最小值。 因此，截斷函數是(33)，其中Struncation是一些常量截斷比例。 因此，截斷距離是動態的，并且隨著深度觀察變得更能夠增加。
6.3.3 并行化
注意，整個TSDF融合計算復雜度近似等于體素更新復雜度，其與體素計數成比例。 每個體素的更新過程是獨立的，這允許我們通過多線程加速過程。我們還將地圖更新和發布過程分離，以保證最新的軌跡規劃地圖的可用性。 一旦深度圖像（可以來自兩個ROI中的任何一個）到來，地圖更新就會運行，而地圖publisher會以10 Hz的恒定頻率將地圖輸出到軌跡規劃。 我們使用互斥鎖來保證共享內存不會沖突。
6.3.4 渲染可視化地圖
雖然自主導航不需要視覺上吸引人的地圖，但我們仍然提供了一個離線運行的地圖渲染模塊，以實現可視化目的。 Marching Cube算法51用于地圖渲染。對于每八個體素，我們將它們的中心形成立方體，并且每個中心點的帶符號距離場(SDF)值近似等于其體素的SDF值。 考慮每個SDF值的符號。 整個立方體有256種不同的可能構成，這意味著有256種不同的方法將立方體分成幾個三角形網格(見圖9)。 樣本重建結果在圖10中可視化。
7.軌跡規劃
暫未看之后內容
8. 實驗結果
