基于復合標志的四旋翼無人機自主降落系統*

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

（1.中北大學機電工程學院，山西 太原 030051；2.江蘇省機器人技術及智能制造裝備工程實驗室（蘇州大學），江蘇 蘇州 215021）

0 引 言

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

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

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

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

1 無人機自主降落系統概述

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

圖1 四旋翼無人機自主降落系統總體框架

圖2 大疆M210V2 無人機及安裝在底部的下視攝像頭（框標注）

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

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

圖3 定義的坐標系

2 無人機著陸標志設計與檢測

當無人機與降落目標距離較近時，利用視覺估計的無人機和降落目標間的相對位姿遠比GPS精確。為了減小由于使用低成本單目攝像頭帶來的不利因素，設計了基于Apriltag視覺基準庫的復合標簽作為著陸標志，如圖4所示。

圖4 著陸標志

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

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

3 基于卡爾曼濾波器的狀態估計

無人機在接近著陸目標的過程中，容易發生著陸目標“丟失”的現象［10］。因此設計卡爾曼濾波器來估計和跟蹤無人機和著陸目標的運動狀態。

3.1 運動方程

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

運動方程定義如下

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

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

3.2 測量方程

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

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

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

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

4 控制系統設計

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

在水平方向上，采用標準的PID 控制器來計算目標加速度，，其中，kp，kd，ki為控制器的固定增益。因為只對水平方向無人機的位置進行控制，因此加速度a在z軸方向上分量為0。根據文獻［11］中建立無人機動力學模型的方法，將期望加速度轉換為無人機的姿態輸入角度指令，從而使得無人機到達目標位置。為保證無人機的平穩飛行，根據無人機的飛行參數限制，在大疆無人機專用模擬器上對kp，kd，ki進行調節。

5 自主降落系統仿真

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

圖5 大疆無人機模擬器

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

圖6 無人機自主降落過程

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

表1 無人機降落誤差統計cm

圖7 無人機自主降落和XY平面著陸點分布

6 結束語

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