基于復(fù)合標(biāo)志的四旋翼無人機(jī)自主降落系統(tǒng)*

袁澤慧，陳國棟，張 亞，王忠言

（1.中北大學(xué)機(jī)電工程學(xué)院，山西 太原 030051；2.江蘇省機(jī)器人技術(shù)及智能制造裝備工程實(shí)驗(yàn)室（蘇州大學(xué)），江蘇 蘇州 215021）

0 引 言

無人機(jī)能否在規(guī)定空間內(nèi)快速、精確降落在地面充電裝置或回收裝置上，是實(shí)現(xiàn)空地機(jī)器人協(xié)同的關(guān)鍵，同時(shí)也是無人機(jī)最具挑戰(zhàn)性的任務(wù)之一。目前最常用的方法是利用高精度的全球定位系統(tǒng)（global positioning system，GPS）和慣性傳感器［1］、運(yùn)動捕捉系統(tǒng)［2，3］、計(jì)算機(jī)視覺［4］來估計(jì)無人機(jī)和降落目標(biāo)之間的相對位姿。但GPS 在室內(nèi)、森林、城市等環(huán)境中信號較弱甚至失效［5］，這成為GPS 的致命缺陷。而運(yùn)動捕捉系統(tǒng)一般僅用于室內(nèi)環(huán)境中。相反，視覺傳感器可以獲得豐富的環(huán)境信息，有效避免了這些問題。同時(shí)，視覺傳感器具有輕便、低功耗、低成本等優(yōu)點(diǎn)，而且在近距離測量精度更高。因此視覺傳感器成為無人機(jī)的必備傳感器之一。

Yang S等人利用單目攝像頭，結(jié)合比例校正并行追蹤與繪制（parallel tracking and mapping，PTAM）技術(shù)，通過ORB（oriented FAST and rotated BRIEF）匹配算法，實(shí)現(xiàn)了無人機(jī)在任意紋理豐富的平臺上降落［6］。Foster C等人則利用下視單目攝像頭，結(jié)合慣性測量單元，建立以無人機(jī)為中心點(diǎn)的下方區(qū)域的概率二維高程地圖，進(jìn)而實(shí)現(xiàn)對安全降落點(diǎn)的檢測［7］。這2種方法都實(shí)現(xiàn)了在非標(biāo)志平臺上的降落，但相較于標(biāo)志平臺的檢測，魯棒性較低，而且所需的降落時(shí)間更長。

H形圖像常作為著陸標(biāo)志在無人機(jī)降落中使用［8，9］，Ling K［10］、Borowczyk A 等人［11］在他們的工作中則采用了AprilTag二維碼［12］作為著陸標(biāo)志。但由于無人機(jī)高度的變化，單個(gè)的視覺標(biāo)簽經(jīng)常會面臨無法檢測到的情況，如當(dāng)無人機(jī)飛行高度較高時(shí)，標(biāo)簽無法被檢測到，或者當(dāng)無人機(jī)逐漸接近地面時(shí)，標(biāo)簽逐漸從無人機(jī)下方攝像頭的視野內(nèi)移出，從而導(dǎo)致無人機(jī)的降落精度降低。為了應(yīng)對著陸目標(biāo)“丟失”的情況，西北工業(yè)大學(xué)張咪等人提出了一種多層嵌套二維編碼的階層降落標(biāo)識［13］，索文凱等人設(shè)計(jì)了一種多圖形組合、多色彩導(dǎo)引、多方法識別的地面合作目標(biāo)［14］。

本文基于大疆M210V2 四旋翼無人機(jī)，搭建了一種基于視覺的無人機(jī)在靜止目標(biāo)上的自主降落系統(tǒng)，其中攝像頭采用低成本的單目攝像頭。為了應(yīng)對著陸目標(biāo)丟失的情況，以及抵消單目攝像頭導(dǎo)致的卷簾效應(yīng)［15］和成像噪聲等不利因素，基于AprilTag 視覺基準(zhǔn)庫設(shè)計(jì)了一復(fù)合著陸標(biāo)志，使得無人機(jī)在著陸過程中的不同高度都能采集到有效的視覺信息。同時(shí)結(jié)合卡爾曼濾波器，對著陸目標(biāo)進(jìn)行實(shí)時(shí)跟蹤。最后，在大疆專用模擬器中對所設(shè)計(jì)的自主降落系統(tǒng)進(jìn)行了仿真，驗(yàn)證了所設(shè)計(jì)的自主降落系統(tǒng)的有效性和精確性。

1 無人機(jī)自主降落系統(tǒng)概述

本文設(shè)計(jì)的四旋翼無人機(jī)自主降落系統(tǒng)的總體框架如圖1 所示。四旋翼無人機(jī)采用大疆M210V2 無人機(jī)，該無人機(jī)配備有大疆FlightAutonomy系統(tǒng)，以不同頻率向外發(fā)布無人機(jī)的位置、姿態(tài)、速度以及加速度等信息；無人機(jī)底部安裝有下視攝像頭，其光軸垂直向下，主要用于對著陸標(biāo)志的識別和檢測；這兩部分構(gòu)成了系統(tǒng)的硬件部分，如圖2所示。

圖1 四旋翼無人機(jī)自主降落系統(tǒng)總體框架

圖2 大疆M210V2 無人機(jī)及安裝在底部的下視攝像頭（框標(biāo)注）

基于卡爾曼濾波器的狀態(tài)估計(jì)模塊主要對無人機(jī)flightAutonomy系統(tǒng)發(fā)布的狀態(tài)信息，以及基于著陸標(biāo)志檢測算法測量得到的無人機(jī)和著陸標(biāo)志間的相對位姿進(jìn)行融合，實(shí)時(shí)對無人機(jī)和著陸目標(biāo)間的運(yùn)動狀態(tài)信息進(jìn)行估計(jì)。在此基礎(chǔ)上，PID 控制器根據(jù)無人機(jī)和著陸目標(biāo)間的相對位置，計(jì)算下一時(shí)刻無人機(jī)的期望加速度；根據(jù)文獻(xiàn)［11］中建立無人機(jī)動力學(xué)模型的方法，將PID 控制器模塊輸出的期望加速度轉(zhuǎn)換為無人機(jī)的姿態(tài)控制指令，通過無人機(jī)姿態(tài)的改變，來控制無人機(jī)飛行到期望位置。這4 個(gè)模塊均由機(jī)載計(jì)算機(jī)來完成。各模塊之間利用機(jī)器人操作系統(tǒng)（robot operation system，ROS）接口進(jìn)行通信［16］。模塊化的結(jié)構(gòu)便于輕松修改或替換各模塊內(nèi)的算法，而無需對其他模塊進(jìn)行更改。因此，本文提出的是一種無人機(jī)自主降落的通用框架，可通過更改對應(yīng)的模塊來適應(yīng)不同的算法或場景。

無人機(jī)自主降落系統(tǒng)中所定義的坐標(biāo)系如圖3 所示。世界｛W｝坐標(biāo)系，采用東-北-天（east-north-up，ENU）坐標(biāo)系表示，其坐標(biāo)原點(diǎn)定義為無人機(jī)起飛時(shí)的位置；機(jī)體坐標(biāo)系｛B｝，其原點(diǎn)固連在四旋翼無人機(jī)的重心上，X軸指向機(jī)頭方向，Y軸指向左側(cè)，Z軸垂直于XY軸向上。攝像頭｛C｝坐標(biāo)系的原點(diǎn)為相機(jī)光心，Z軸垂直于相機(jī)平面，沿著光軸方向，X軸和Y軸分別平行于圖像平面，其中，X軸指向右側(cè)，而Y軸由右手定則所確定。

圖3 定義的坐標(biāo)系

2 無人機(jī)著陸標(biāo)志設(shè)計(jì)與檢測

當(dāng)無人機(jī)與降落目標(biāo)距離較近時(shí)，利用視覺估計(jì)的無人機(jī)和降落目標(biāo)間的相對位姿遠(yuǎn)比GPS精確。為了減小由于使用低成本單目攝像頭帶來的不利因素，設(shè)計(jì)了基于Apriltag視覺基準(zhǔn)庫的復(fù)合標(biāo)簽作為著陸標(biāo)志，如圖4所示。

圖4 著陸標(biāo)志

結(jié)合本項(xiàng)目中地面機(jī)器人的尺寸，著陸標(biāo)志的總體大小為90 cm×90 cm，由5個(gè)不同大小的Apriltag標(biāo)簽組成，其中中間較小的標(biāo)簽為16 cm×16 cm，而周圍4個(gè)標(biāo)簽的大小為28 cm×28 cm。復(fù)合標(biāo)簽的使用可以使得攝像機(jī)無論是在高空和低空都能探測到著陸標(biāo)志，從而降低了著陸目標(biāo)丟失的可能性。當(dāng)無人機(jī)離著陸目標(biāo)較遠(yuǎn)時(shí)，可以檢測到較大的標(biāo)簽，當(dāng)無人機(jī)在降落過程中不斷接近著陸目標(biāo)時(shí)，較大的標(biāo)簽逐漸移出攝像頭的視野范圍，但中間較小的標(biāo)簽仍然在其視野范圍內(nèi)，這樣就保證了無人機(jī)無論是在較遠(yuǎn)處還是近處，著陸目標(biāo)都位于攝像頭的視角范圍之內(nèi)。

著陸目標(biāo)檢測算法設(shè)計(jì)中，本文使用了基于Apriltag2的ROS封裝器［17］，與主體算法相接。下視攝像頭將采集到的640 ×480的原圖像Iraw輸入到AprilTag2，并利用AprilTag2算法對著陸標(biāo)志中的標(biāo)簽進(jìn)行識別和標(biāo)記。對于識別到的標(biāo)簽，將其ID 標(biāo)記為i，假設(shè)至少有一個(gè)標(biāo)簽被檢測到，利用OpenCV 中的PnP（perspective n point）算法估計(jì)得到著陸標(biāo)志在下視攝像頭坐標(biāo)系中的位置和姿態(tài)。由于多個(gè)標(biāo)簽之間的相對位姿已知或可事先進(jìn)行標(biāo)定，在對著陸目標(biāo)進(jìn)行檢測時(shí)，可提取所有標(biāo)簽的信息，以及標(biāo)簽間的位姿約束，來估計(jì)無人機(jī)和著陸目標(biāo)時(shí)的相對位置和姿態(tài)。因此相對于單個(gè)標(biāo)簽，利用復(fù)合標(biāo)簽估計(jì)得到的相對位姿更加精確，從而有利于提高無人機(jī)的自主降落精度。

3 基于卡爾曼濾波器的狀態(tài)估計(jì)

無人機(jī)在接近著陸目標(biāo)的過程中，容易發(fā)生著陸目標(biāo)“丟失”的現(xiàn)象［10］。因此設(shè)計(jì)卡爾曼濾波器來估計(jì)和跟蹤無人機(jī)和著陸目標(biāo)的運(yùn)動狀態(tài)。

3.1 運(yùn)動方程

為方便將該模塊日后移植到移動目標(biāo)上的降落中，濾波器狀態(tài)量包含了無人機(jī)和著陸目標(biāo)的運(yùn)動狀態(tài)，狀態(tài)量為其中分別為無人機(jī)在世界坐標(biāo)系中的位置、速度和加速度，而則為著陸目標(biāo)在世界坐標(biāo)系中的位置、速度和加速度。顯然，在本文中著陸目標(biāo)的速度和加速度都為0。

運(yùn)動方程定義如下

式中vt，at分別為速度和加速度。設(shè)無人機(jī)加速度（）T的一次微分為高斯白噪聲，其功率譜密度矩陣為qwI3，其中qw為功率譜密度參數(shù)，I3為3 ×3的單位陣。對運(yùn)動方程進(jìn)行離散化，得到其離散形式xk＋1＝Fxk＋wk，其中

式中 Δt為k＋1時(shí)刻和k時(shí)刻之間的時(shí)間間隔，I3為3 ×3的單位矩陣，03為3 ×3 的零矩陣，?為兩矩陣間的克羅內(nèi)克積，而wk為高斯白噪聲，其協(xié)方差矩陣定義為

3.2 測量方程

如前所述，本文中的測量數(shù)據(jù)主要來自于：1）無人機(jī)的FlightAutonomy系統(tǒng)；2）機(jī)載攝像頭估計(jì)得到的著陸目標(biāo)和無人機(jī)間的相對位置和姿態(tài)。當(dāng)濾波器接收到任意一組數(shù)據(jù)時(shí)，濾波器對無人機(jī)的狀態(tài)進(jìn)行預(yù)測和更新。雖然測量數(shù)據(jù)來源不同，但本文中采用了統(tǒng)一的測量模型，即

式中vk為高斯白噪聲，其協(xié)方差矩陣為Vk。對于不同的測量數(shù)據(jù)，其Hk和vk不同。

大疆M210V2 FlightAutonomy 系統(tǒng)融合了GPS、慣性測量單元等多傳感器信息，可提供無人機(jī)在世界坐標(biāo)系｛W｝下的位置、速度和加速度。所以其測量矩陣Hk＝［I909×9］。

當(dāng)無人機(jī)離著陸目標(biāo)較近時(shí)，機(jī)載下視攝像頭可以測量著陸目標(biāo)在相機(jī)坐標(biāo)系下的位置。在本文中，下視攝像頭固定安裝在無人機(jī)底部，所以相機(jī)｛C｝坐標(biāo)系和無人機(jī)｛B｝坐標(biāo)系之間的變換關(guān)系可提前進(jìn)行標(biāo)定，進(jìn)而可以得到。利用無人機(jī)FlightAutonomy 系統(tǒng)提供的當(dāng)前無人機(jī)的姿態(tài)信息，結(jié)合矩陣變換關(guān)系得到其在世界坐標(biāo)系下的表示，即，所以其測量矩陣Hk＝［I303×6-I303×6］。

4 控制系統(tǒng)設(shè)計(jì)

本文的控制目標(biāo)是使無人機(jī)的位置無限接近著陸目標(biāo)中心的位置，即使相對位置u＝-盡可能小。將無人機(jī)降落分為2 個(gè)階段，首先調(diào)節(jié)水平方向上的誤差u‖，當(dāng)無人機(jī)與著陸目標(biāo)中心點(diǎn)間距離小于一定閾值時(shí)，即｜u‖｜＜Δd時(shí)，控制無人機(jī)以一定的速度垂直且勻速下落，當(dāng)無人機(jī)接觸到地面后，關(guān)閉無人機(jī)，降落成功。

在水平方向上，采用標(biāo)準(zhǔn)的PID 控制器來計(jì)算目標(biāo)加速度，，其中，kp，kd，ki為控制器的固定增益。因?yàn)橹粚λ椒较驘o人機(jī)的位置進(jìn)行控制，因此加速度a在z軸方向上分量為0。根據(jù)文獻(xiàn)［11］中建立無人機(jī)動力學(xué)模型的方法，將期望加速度轉(zhuǎn)換為無人機(jī)的姿態(tài)輸入角度指令，從而使得無人機(jī)到達(dá)目標(biāo)位置。為保證無人機(jī)的平穩(wěn)飛行，根據(jù)無人機(jī)的飛行參數(shù)限制，在大疆無人機(jī)專用模擬器上對kp，kd，ki進(jìn)行調(diào)節(jié)。

5 自主降落系統(tǒng)仿真

為了驗(yàn)證提出的四旋翼無人機(jī)自主降落控制系統(tǒng)，使用大疆專用的硬件在環(huán)（hardware in the loop，HITL）模擬器對其進(jìn)行仿真驗(yàn)證，如圖5 所示，其中圖5（a）為模擬器控制面板，可以設(shè)置無人機(jī)起飛初始位置、風(fēng)速等環(huán)境信息，圖5（b）為無人機(jī)飛行界面。

圖5 大疆無人機(jī)模擬器

四旋翼無人機(jī)自主降落系統(tǒng)的飛行過程如圖6 所示。為飛行安全起見，在無人機(jī)起飛前確定電池電量充足，以及GPS信號是否能正常接收，如滿足條件，將當(dāng)前無人機(jī)所在位置設(shè)置為世界坐標(biāo)系｛W｝的原點(diǎn)，無人機(jī)執(zhí)行起飛動作，并飛行到預(yù)設(shè)的目標(biāo)點(diǎn)。當(dāng)?shù)竭_(dá)目標(biāo)點(diǎn)后，啟動基于卡爾曼濾波的狀態(tài)估計(jì)器和AprilTag 算法對著陸目標(biāo)進(jìn)行檢測。當(dāng)檢測到著陸目標(biāo)標(biāo)志后，無人機(jī)定高飛行，在水平方向上接近著陸目標(biāo)。反之如果沒有檢測到著陸目標(biāo)標(biāo)志，對著陸目標(biāo)進(jìn)行網(wǎng)格化搜索直到檢測到著陸目標(biāo)。當(dāng)無人機(jī)與著陸目標(biāo)中心點(diǎn)水平誤差小于預(yù)設(shè)的閾值后，本文中為0.08 m，無人機(jī)在垂直方向上以一定的速度勻速降落，無人機(jī)到達(dá)地面后，關(guān)掉無人機(jī)，自主降落完成。

圖6 無人機(jī)自主降落過程

在模擬實(shí)驗(yàn)中，為最大限度接近真實(shí)飛行環(huán)境，將模擬環(huán)境設(shè)置為有風(fēng)環(huán)境，其中東風(fēng)6 m／s，北風(fēng)6 m／s，對應(yīng)4級風(fēng)速。無人機(jī)從不同高度、不同位置接近著陸目標(biāo)，共進(jìn)行了100次硬件在環(huán)模擬仿真實(shí)驗(yàn)。結(jié)果如圖7 所示。從圖7（a）中可以看出，無人機(jī)最終都降落在了著陸目標(biāo)中心點(diǎn)附近，進(jìn)一步根據(jù)圖7（b）可知，降落誤差均位于以著陸目標(biāo)為圓心，0.08 m為半徑的圓內(nèi)，該誤差值與預(yù)設(shè)的水平方向誤差閾值相同。同時(shí)在仿真中發(fā)現(xiàn)無人機(jī)在垂直下落過程中水平誤差并沒有超過閾值，說明無人機(jī)在垂直下落過程中，得益于大疆無人機(jī)控制系統(tǒng)的穩(wěn)定性能，其在水平方向的誤差并沒有增大。降落誤差進(jìn)一步統(tǒng)計(jì)如表1 所示。從表1中可以看出，無人機(jī)在X和Y方向的平均降落誤差均小于5 cm，且最大誤差不超過8 cm，實(shí)現(xiàn)了高精度的自主降落。

表1 無人機(jī)降落誤差統(tǒng)計(jì)cm

圖7 無人機(jī)自主降落和XY平面著陸點(diǎn)分布

6 結(jié)束語

針對小型無人機(jī)的高精度自主降落問題，在大疆M210V2基礎(chǔ)上，搭建了一種基于視覺的無人機(jī)自主降落系統(tǒng)，其中機(jī)載攝像頭采用了低成本的單目攝像頭。為應(yīng)對使用低成本攝像頭帶來的不利因素，設(shè)計(jì)了基于AprilTag的多標(biāo)簽著陸標(biāo)志，并建立基于卡爾曼濾波器的狀態(tài)估計(jì)模型對無人機(jī)和著陸目標(biāo)的運(yùn)動狀態(tài)進(jìn)行實(shí)時(shí)預(yù)測和跟蹤。在此基礎(chǔ)上，通過PID 控制器計(jì)算無人機(jī)下一時(shí)刻的期望加速度，并將該加速度轉(zhuǎn)換為無人機(jī)的姿態(tài)指令。在大疆專用模擬器下，對該系統(tǒng)進(jìn)行了100 次硬件在環(huán)模擬仿真，仿真結(jié)果表明在4級風(fēng)速的模擬環(huán)境中，無人機(jī)在X和Y方向的平均降落誤差均小于5 cm，最大誤差不超過8 cm。在下一步工作中，將進(jìn)行無人機(jī)在戶外自主降落的實(shí)驗(yàn)，進(jìn)一步驗(yàn)證該系統(tǒng)的可行性和精確性，從而為空地協(xié)同系統(tǒng)的實(shí)現(xiàn)提供基礎(chǔ)。