基于Apriltag算法的在線恢復(fù)深度的無人機(jī)單目SLAM系統(tǒng)

安晨亮，高喜天，冀 東

（天津工業(yè)大學(xué) 電氣工程與自動化學(xué)院，天津300387）

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

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

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

1 系統(tǒng)概述

框架中5 個(gè)模塊的關(guān)系如圖1所示。深度求解模塊首先被設(shè)計(jì)為利用Apriltag2 算法來計(jì)算比例尺信息。初始化單目模式的ORB-SLAM2，然后將其與相機(jī)深度融合以提供狀態(tài)估計(jì)結(jié)果。引入了軌跡計(jì)劃模塊以生成所需軌跡，并且控制器模塊通過融合實(shí)時(shí)姿態(tài)信息來計(jì)算用于安全、準(zhǔn)確地為無人機(jī)提供運(yùn)動的輸入命令。最后，大疆自動駕駛儀通過硬件接口接收控制器發(fā)送的輸入命令?？蚣苤兴心K之間的通信都是通過ROS 接口進(jìn)行的。

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

2 自主式單目無人機(jī)控制系統(tǒng)

2.1 框架轉(zhuǎn)換和深度求解

該框架的第一個(gè)模塊是通過合并Apriltag2 和ORB-SLAM2 獲得有關(guān)單目相機(jī)的深度信息。位置、姿態(tài)的發(fā)布和轉(zhuǎn)換模塊是在ROS 環(huán)境設(shè)計(jì)的。無人機(jī)上安裝了英特爾實(shí)感D435i 傳感器，并且所提的框架中僅使用單目相機(jī)，相機(jī)坐標(biāo)系表示為Fc，F(xiàn)c在初始時(shí)刻時(shí)表示為Fc0，F(xiàn)b表示無人機(jī)機(jī)體坐標(biāo)系，F(xiàn)b在初始時(shí)刻t=0 時(shí)表示為Fb0。

參考坐標(biāo)系定義在Apriltag2 的標(biāo)簽上，用Fw表示。利用標(biāo)簽檢測算法，在Fc下表示的坐標(biāo)系Fw的旋轉(zhuǎn)矩陣和平移向量都是通過Apriltag2姿態(tài)計(jì)算算法獲得的。表示坐標(biāo)系Fb在Fw下的轉(zhuǎn)換關(guān)系，同樣的表示坐標(biāo)系Fc在Fw下的轉(zhuǎn)換關(guān)系，表示預(yù)校準(zhǔn)的相機(jī)和無人機(jī)之間的轉(zhuǎn)換關(guān)系。根據(jù)坐標(biāo)系轉(zhuǎn)換原理，可以通過計(jì)算和得到，計(jì)算過程如下所示：

系統(tǒng)啟動時(shí)，Apriltag2 和ORB-SLAM2 都會啟動，t′表示ORB-SLAM2 初始化完成的瞬間。我們假設(shè)啟動系統(tǒng)時(shí)標(biāo)簽位于相機(jī)視場中。通過對Apriltag2進(jìn)行姿態(tài)測量，可以得到從時(shí)間ti到Fc0的攝像機(jī)幀的變化量，如下所示：

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

2.2 狀態(tài)估計(jì)模塊

結(jié)合已建立的單目SLAM（例如ORB-SLAM2），將其用于UAV 平臺，它可以提供可靠、準(zhǔn)確的UAV狀態(tài)估計(jì)和工作區(qū)映射結(jié)果。

在ORB-SLAM2 中，僅運(yùn)動包調(diào)整（BA）可以將攝像機(jī)幀的方向和位置從Fc0最佳化為當(dāng)前時(shí)間t，從而使匹配的3-D 點(diǎn)Xi∈R3在Fc0以下和圖像關(guān)鍵點(diǎn)與i∈c 之間的重投影誤差最小。所有匹配項(xiàng)的集合為

式中：r（·）是魯棒的Huber 成本函數(shù)；∑是與關(guān)鍵點(diǎn)規(guī)模相關(guān)的協(xié)方差矩陣。pm（·）的定義如下：

式中：校準(zhǔn)的相機(jī)參數(shù)（fx，fy）是縮放的焦距；（cx，cy）是圖像上的主要特征點(diǎn)。

原始的ORB-SLAM2 主要具有3 個(gè)并行線程：跟蹤，局部映射和循環(huán)關(guān)閉。但是，生成的姿態(tài)信息不會實(shí)時(shí)發(fā)布。為了解決無人機(jī)無法直接獲得深度融合ORB-SLAM2 提供的狀態(tài)估計(jì)的問題，設(shè)計(jì)了一種姿態(tài)發(fā)布轉(zhuǎn)換模塊，如圖2所示。

圖2 發(fā)布和轉(zhuǎn)換模塊Fig.2 Release and conversion module

在圖2 中，姿態(tài)信息是通過利用ROS 發(fā)布者/訂閱者工具進(jìn)行傳輸?shù)?。利用轉(zhuǎn)換節(jié)點(diǎn)，可以根據(jù)攝像機(jī)/UAV 坐標(biāo)系的關(guān)系從深度融合攝像機(jī)的姿態(tài)中獲得UAV 姿態(tài)，如圖3所示。

圖3 無人機(jī)和相機(jī)之間的坐標(biāo)系關(guān)系Fig.3 Frames relationship of the drone and onboard camera

2.3 軌跡規(guī)劃模塊

為了通過無人機(jī)完成復(fù)雜的任務(wù)，通常會結(jié)合平臺大小、目標(biāo)跟蹤、能耗等因素，組合適當(dāng)?shù)倪\(yùn)動軌跡計(jì)劃。

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

在式（12）中，引入?yún)?shù)w 來設(shè)置所需的線性和角速度，r 與曲線的大小有關(guān)，并且參數(shù)p/2 涉及與無人機(jī)起降過程的合理連接。

2.4 運(yùn)動控制器

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

控制過程如圖4所示。位置和速度誤差都可以通過計(jì)劃模塊和當(dāng)前狀態(tài)估計(jì)模塊中的所需軌跡來計(jì)算。

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

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

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

然后分別計(jì)算輸入的俯仰角和側(cè)傾角為

為了穩(wěn)定偏航角跟蹤誤差的零平衡，應(yīng)用比例控制器來計(jì)算所需的偏航率輸入：

2.5 硬件接口

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

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

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

框架中的所有模塊都以ROS 節(jié)點(diǎn)的形式執(zhí)行通信，這些節(jié)點(diǎn)是Apriltag 節(jié)點(diǎn)、ORB-SLAM2 節(jié)點(diǎn)、姿態(tài)轉(zhuǎn)換節(jié)點(diǎn)、深度節(jié)點(diǎn)和大疆SDK 節(jié)點(diǎn)。節(jié)點(diǎn)關(guān)系如圖6所示。

啟動系統(tǒng)后，Apriltag 節(jié)點(diǎn)和ORB-SLAM 節(jié)點(diǎn)均立即開始工作。通過深度節(jié)點(diǎn)，利用接收到的上述2 個(gè)節(jié)點(diǎn)的信息輸入，獲得單目相機(jī)的深度信息。對于比例恢復(fù)的姿態(tài)，UAV 的狀態(tài)信息是通過姿態(tài)轉(zhuǎn)換節(jié)點(diǎn)獲得的。大疆SDK 節(jié)點(diǎn)一旦接收到相應(yīng)的姿態(tài)信息，便向UAV 發(fā)送控制信號。

在大疆SDK 節(jié)點(diǎn)中，操作流程如圖7所示。在初始化過程中，需要啟用4 個(gè)電機(jī)并掌握控制權(quán)限，以便機(jī)載計(jì)算機(jī)可以操縱四旋翼。電動機(jī)啟動后，需要以50 Hz 的頻率向無人機(jī)發(fā)送控制命令，以進(jìn)行穩(wěn)定的飛行控制。在無人機(jī)著陸后釋放控制權(quán)之前，應(yīng)先禁用電動機(jī)。

圖6 ROS 中的節(jié)點(diǎn)關(guān)系Fig.6 Node diagram in ROS

圖7 大疆SDK 節(jié)點(diǎn)中的運(yùn)行流程Fig.7 Operating flow chart of DJI SDK node

3 實(shí)驗(yàn)結(jié)果

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

圖8 實(shí)驗(yàn)設(shè)備Fig.8 Experimental setup

D435i 攝像機(jī)的校準(zhǔn)參數(shù)為fx：615.9；fy：616.1；cx：322.4；cy：240.4。外在參數(shù)測量為

此外，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］）。標(biāo)簽的規(guī)格和位置為｛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｝。

在實(shí)驗(yàn)中，無人機(jī)分別在執(zhí)行軌跡跟蹤任務(wù)之前和之后起飛和降落。在軌跡跟蹤任務(wù)中，無人機(jī)的運(yùn)動軌跡如圖9所示，姿態(tài)演變?nèi)鐖D10所示，可以看出當(dāng)前位置和所需位置具有很好的一致性。

在Fc0坐標(biāo)系下表示的橫擺角、俯仰角和偏航角的演變?nèi)鐖D11所示，而推力f 的輸出如圖12所示。

實(shí)驗(yàn)結(jié)果表明，無人機(jī)在深度恢復(fù)后可以通過單目ORB-SLAM2 算法成功完成軌跡跟蹤任務(wù)，所提出的框架可以實(shí)時(shí)執(zhí)行高層任務(wù)。

圖9 無人機(jī)在跟蹤過程中的運(yùn)動軌跡Fig.9 Motion trajectories of the UAV during the tracking process

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

圖11 無人機(jī)在整個(gè)過程中的姿態(tài)演變Fig.11 Attitude evolution of the UAV during the entire process

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

4 結(jié)語

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