安晨亮，高喜天，冀 東

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

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

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

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

1 系統(tǒng)概述

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

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

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

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

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

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

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

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

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

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

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

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

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

原始的ORB-SLAM2 主要具有3 個并行線程：跟蹤，局部映射和循環(huán)關(guān)閉。但是，生成的姿態(tài)信息不會實時發(fā)布。為了解決無人機無法直接獲得深度融合ORB-SLAM2 提供的狀態(tài)估計的問題，設(shè)計了一種姿態(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é)點，可以根據(jù)攝像機/UAV 坐標(biāo)系的關(guān)系從深度融合攝像機的姿態(tài)中獲得UAV 姿態(tài)，如圖3所示。

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

2.3 軌跡規(guī)劃模塊

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

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

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

2.4 運動控制器

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

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

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

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

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

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

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

2.5 硬件接口

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

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

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

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

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

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

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

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

3 實驗結(jié)果

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

圖8 實驗設(shè)備Fig.8 Experimental setup

D435i 攝像機的校準(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｝。

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

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

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

圖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 無人機在整個過程中的姿態(tài)演變Fig.11 Attitude evolution of the UAV during the entire process

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

4 結(jié)語

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