基于Apriltag算法的在線恢復深度的無人機單目SLAM系統

安晨亮，高喜天，冀 東

（天津工業大學 電氣工程與自動化學院，天津300387）

近年來，無人駕駛飛機在視覺伺服[1-2]、非線性控制[3]、SLAM 等許多研究領域受到了廣泛的關注。大疆Matrice100 由于其出色的穩定性，對開發的再次支持[4]和強大的耐力而被廣泛用于無人機研究領域。例如軌跡跟蹤控制[5]，目標檢測和跟蹤[6]，路徑規劃[7]，基于視覺的導航[8-9]等。在室外環境中，大疆無人機可以穩定多旋翼的位置，并通過全球定位系統（GPS）提供正確的姿態和速度信號估計。但是，在室內環境下使用大疆Guidance[10]時，它無法直接融合來自外部傳感器的任何數據，因為缺少GPS 信號。因此，設計無人機的框架以提供狀態和控制信息非常重要，該信息可以獲取無法訪問GPS 的速度和姿態。

單目相機具有結構簡單、成本低廉、采樣數據充足等優點，在SLAM 研究領域引起了廣泛的關注。利用單目視覺，已經有許多關于狀態估計，里程表和SLAM 的完整著作，包括PTAM[11]、SVO[12]、LSDSLAM[13]和VINS-Mono[14]。對于單目視覺和無人機上的SLAM 應用而言，快速完成初始化并準確恢復真實尺度信息至關重要。為了解決初始化問題，已經提出了許多方法，例如文獻[15]和文獻[16]。在文獻[15]中設計了線性估計器初始化模式，它利用了來自短期慣性測量單元（IMU）預積分的相對旋轉，文獻[16]中針對單目視覺慣性初始化問題引入了封閉形式的解決方案。

使用ORB-SLAM2[17]，僅通過使用單目視覺就可以快速完成初始化過程，這意味著可以滿足無人機的狀態估計和建圖需求。但是，由于無法從單目相機恢復深度信息，因此，地圖的比例和預測的軌跡是未知的。對于無人機上的單目SLAM 應用，應該完成確定有關單目相機的比例尺信息的過程。此外，Apriltag2[18]最近因其穩定性和準確性而備受關注，可用于單目SLAM 的尺度恢復。在文獻[19]中，基于具有飽和位置的四旋翼無人機的線性化動力學，開發了一種非線性控制器。文獻[20]中將反推和滑模技術都應用于無人機軌跡跟蹤。在文獻[21]中開發了一種用于軌跡跟蹤任務的四旋翼無人機的幾何控制器，它可以控制欠驅動的四旋翼無人機使用4 個推力輸入來穩定6 個平移和旋轉自由度，同時漸近地跟蹤4 個由其位置和航向組成的輸出。

1 系統概述

框架中5 個模塊的關系如圖1所示。深度求解模塊首先被設計為利用Apriltag2 算法來計算比例尺信息。初始化單目模式的ORB-SLAM2，然后將其與相機深度融合以提供狀態估計結果。引入了軌跡計劃模塊以生成所需軌跡，并且控制器模塊通過融合實時姿態信息來計算用于安全、準確地為無人機提供運動的輸入命令。最后，大疆自動駕駛儀通過硬件接口接收控制器發送的輸入命令。框架中所有模塊之間的通信都是通過ROS 接口進行的。

圖1 單目視覺無人機控制框架概述Fig.1 Overview of monocular vision UAV control framework

2 自主式單目無人機控制系統

2.1 框架轉換和深度求解

該框架的第一個模塊是通過合并Apriltag2 和ORB-SLAM2 獲得有關單目相機的深度信息。位置、姿態的發布和轉換模塊是在ROS 環境設計的。無人機上安裝了英特爾實感D435i 傳感器，并且所提的框架中僅使用單目相機，相機坐標系表示為Fc，Fc在初始時刻時表示為Fc0，Fb表示無人機機體坐標系，Fb在初始時刻t=0 時表示為Fb0。

參考坐標系定義在Apriltag2 的標簽上，用Fw表示。利用標簽檢測算法，在Fc下表示的坐標系Fw的旋轉矩陣和平移向量都是通過Apriltag2姿態計算算法獲得的。表示坐標系Fb在Fw下的轉換關系，同樣的表示坐標系Fc在Fw下的轉換關系，表示預校準的相機和無人機之間的轉換關系。根據坐標系轉換原理，可以通過計算和得到，計算過程如下所示：

系統啟動時，Apriltag2 和ORB-SLAM2 都會啟動，t′表示ORB-SLAM2 初始化完成的瞬間。我們假設啟動系統時標簽位于相機視場中。通過對Apriltag2進行姿態測量，可以得到從時間ti到Fc0的攝像機幀的變化量，如下所示：

此外，在時間t′處，可以在ORB-SLAM2 的輸出下獲得從Fc0到時間t′的攝像機坐標系的縮放平移矢量。因此，通過計算相同相機平移矢量的上述兩個不同表達式之間的比率，可以恢復晦澀難懂的ORB-SLAM2 的比例因子s：

2.2 狀態估計模塊

結合已建立的單目SLAM（例如ORB-SLAM2），將其用于UAV 平臺，它可以提供可靠、準確的UAV狀態估計和工作區映射結果。

在ORB-SLAM2 中，僅運動包調整（BA）可以將攝像機幀的方向和位置從Fc0最佳化為當前時間t，從而使匹配的3-D 點Xi∈R3在Fc0以下和圖像關鍵點與i∈c 之間的重投影誤差最小。所有匹配項的集合為

式中：r（·）是魯棒的Huber 成本函數；∑是與關鍵點規模相關的協方差矩陣。pm（·）的定義如下：

式中：校準的相機參數（fx，fy）是縮放的焦距；（cx，cy）是圖像上的主要特征點。

原始的ORB-SLAM2 主要具有3 個并行線程：跟蹤，局部映射和循環關閉。但是，生成的姿態信息不會實時發布。為了解決無人機無法直接獲得深度融合ORB-SLAM2 提供的狀態估計的問題，設計了一種姿態發布轉換模塊，如圖2所示。

圖2 發布和轉換模塊Fig.2 Release and conversion module

在圖2 中，姿態信息是通過利用ROS 發布者/訂閱者工具進行傳輸的。利用轉換節點，可以根據攝像機/UAV 坐標系的關系從深度融合攝像機的姿態中獲得UAV 姿態，如圖3所示。

圖3 無人機和相機之間的坐標系關系Fig.3 Frames relationship of the drone and onboard camera

2.3 軌跡規劃模塊

為了通過無人機完成復雜的任務，通常會結合平臺大小、目標跟蹤、能耗等因素，組合適當的運動軌跡計劃。

在本框架中，和文獻[3]類似，以伯努利曲線為例，將伯努利曲線作為期望軌跡，因為其難以被跟蹤的特性，可以很好地評估后續跟蹤控制模塊的性能。伯努利曲線的參數方程如下所示：

在式（12）中，引入參數w 來設置所需的線性和角速度，r 與曲線的大小有關，并且參數p/2 涉及與無人機起降過程的合理連接。

2.4 運動控制器

對于大疆無人機而言，不支持期望總時刻的輸入，推力fu（t）、俯仰角qu（t）、橫擺角fu（t）、偏航率是唯一可用的輸入。為了應對這種情況，在框架中采用了基于SE（3）控制方法的幾何控制器[21]來實現復雜的軌跡跟蹤任務。

控制過程如圖4所示。位置和速度誤差都可以通過計劃模塊和當前狀態估計模塊中的所需軌跡來計算。

圖4 控制流程Fig.4 Control flow chart

ep（t）表示期望位置和當前位置之間的誤差向量；ev（t）表示期望速度和當前速度之間的誤差向量；m 是整個系統的總重量。對角增益矩陣表示為Kp和Kv。然后，所需推力fu（t）計算公式為

如下提供了計算控制器的期望姿態輸入Rd（t）的方法：

然后分別計算輸入的俯仰角和側傾角為

為了穩定偏航角跟蹤誤差的零平衡，應用比例控制器來計算所需的偏航率輸入：

2.5 硬件接口

框架中的最后一個模塊是硬件接口，如圖5所示。在收到控制輸入后，接口模塊將通過硬件連接將控制命令發送到大疆無人機。

圖5 框架中的硬件接口Fig.5 Hardware interface of the framework

大疆經緯100（M100）無人機用作運動平臺，因特爾D435i 設備用作單目相機以捕獲圖像信息，并且在飛機上安裝了具有Ubuntu 16.04 的因特爾NUC（8i7BEH）以發送控制指令。作為地面站，個人計算機還配備有Ubuntu 16.04，可通過安全外殼協議（SSH）遠程終端實時掌握機載計算機的狀態。USBTTL 和UART 電纜都是M100 和因特爾NUC 之間的通信通道，但是遠程終端相對獨立于板載控制。在基于ROS（Kinetic）的Ubuntu 16.04 上運行的大疆SDK（OSDK 3.9）是用于M100 開發的必不可少的工具包。

框架中的所有模塊都以ROS 節點的形式執行通信，這些節點是Apriltag 節點、ORB-SLAM2 節點、姿態轉換節點、深度節點和大疆SDK 節點。節點關系如圖6所示。

啟動系統后，Apriltag 節點和ORB-SLAM 節點均立即開始工作。通過深度節點，利用接收到的上述2 個節點的信息輸入，獲得單目相機的深度信息。對于比例恢復的姿態，UAV 的狀態信息是通過姿態轉換節點獲得的。大疆SDK 節點一旦接收到相應的姿態信息，便向UAV 發送控制信號。

在大疆SDK 節點中，操作流程如圖7所示。在初始化過程中，需要啟用4 個電機并掌握控制權限，以便機載計算機可以操縱四旋翼。電動機啟動后，需要以50 Hz 的頻率向無人機發送控制命令，以進行穩定的飛行控制。在無人機著陸后釋放控制權之前，應先禁用電動機。

圖6 ROS 中的節點關系Fig.6 Node diagram in ROS

圖7 大疆SDK 節點中的運行流程Fig.7 Operating flow chart of DJI SDK node

3 實驗結果

在本節中，將提供實驗結果，以基于具有深度恢復能力的單目SLAM 來驗證所提出框架的有效性。實驗場景如圖8（a）所示，實驗平臺為大疆 M100無人機，配備了因特爾NUC 機載計算機和因特爾實感單目相機，如圖8（b）所示，圖8（c）為在執行軌跡跟蹤任務期間捕獲圖像的特征點信息。此外，在https：//github.com/TGUMobileVision/MonocularUAV上提供了有關框架的代碼。

圖8 實驗設備Fig.8 Experimental setup

D435i 攝像機的校準參數為fx：615.9；fy：616.1；cx：322.4；cy：240.4。外在參數測量為

此外，Kp=diag（［0.09，0.09，0.1］），ky=2.8，r=1.0，w=0.0042，Kv=diag（［0.09，0.09，0.12］）。標簽的規格和位置為｛id：1，size：0.143，x：1.8100，y：0.0000，z：0.0，qw：0.707，qx：0.0，qy：0.0，qz：-0.707｝。

在實驗中，無人機分別在執行軌跡跟蹤任務之前和之后起飛和降落。在軌跡跟蹤任務中，無人機的運動軌跡如圖9所示，姿態演變如圖10所示，可以看出當前位置和所需位置具有很好的一致性。

在Fc0坐標系下表示的橫擺角、俯仰角和偏航角的演變如圖11所示，而推力f 的輸出如圖12所示。

實驗結果表明，無人機在深度恢復后可以通過單目ORB-SLAM2 算法成功完成軌跡跟蹤任務，所提出的框架可以實時執行高層任務。

圖9 無人機在跟蹤過程中的運動軌跡Fig.9 Motion trajectories of the UAV during the tracking process

圖10 無人機在跟蹤過程中的位置演變Fig.10 Position evolution of the UAV during the tracking process

圖11 無人機在整個過程中的姿態演變Fig.11 Attitude evolution of the UAV during the entire process

圖12 整個過程中無人機推力的演變Fig.12 Evolution of UAV thrust during the entire process

4 結語

本文通過利用具有深度恢復的單目狀態估計，為開源無人機設計了可擴展框架。在計算單目SLAM 的深度信息后，無人機使用狀態估計提供的姿態信息成功完成軌跡跟蹤和自主著陸任務。在提出的框架中，深度恢復和軌跡跟蹤任務在無人機飛行過程中自動完成。將來我們會努力將附加傳感器的優勢（例如IMU 和LiDAR）帶入該框架，以減少單目SLAM 中的比例尺漂移并提高姿態估計精度。