一種小型四軸飛行器目標跟蹤控制算法

顧兆軍，陳 輝，，王家亮，高 冰，

(1.中國民航大學 信息安全測評中心，天津 300300；2.中國民航大學 計算機科學與技術學院，天津 300300)

小型無人機是一種典型的人工智能空中機器人。其具有成本低、體積小、結構簡單、可對目標進行移動監控等特點，可以有效解決視覺場景“看不清”問題，所以在智慧監控、物流、農業植保、偵查、救援、編隊飛行[1-3]等方面都有著廣泛的應用。最常用的小型無人機是四軸飛行器。近年來，國內外許多高校以及研究機構針對四軸飛行器均開展很多研究工作，如目標跟蹤[4-8]、目標檢測[9-10]、路徑規劃[11]、穩態飛行控制[12]、避障[12-13]、姿態調整[14]、無人機網絡資源優化[15]等方面，并且已經取得顯著的技術成果。其中，目標檢測與跟蹤是小型無人機的重要任務之一，為了能夠持續跟蹤目標、獲取目標信息，無人機執行任務的實時性和飛行穩定性是兩個主要指標。

四軸飛行器的目標檢測與跟蹤是指接收無人機傳回的圖片幀，處理后標記出識別出的物體類別，并跟蹤選定目標飛行。現有的目標檢測算法主要包括one-stage和two-stage兩種。two-stage檢測算法一般分兩步進行，先獲取候選區域再進行分類，比如循環卷積神經網絡(Region-based CNN，R-CNN)[16]系列目標檢測算法；而one-stage則一步到位，不用再單獨獲取候選區域，常見的有SSD[17]、YOLO[18]等。現有的基于計算機視覺的目標跟蹤算法一般可以分為4種：基于目標模型建模的方法、基于搜索的方法、基于相關濾波的跟蹤算法和深度學習方法。前兩種屬于早期的目標跟蹤算法，其沒有將背景信息考慮在內，在目標運動或被遮擋后容易跟蹤失敗，而且其跟蹤算法執行速度慢，無法滿足實時性要求。隨著深度學習的廣泛應用，深度學習方法被應用到目標跟蹤中，但在目標跟蹤過程中，正樣本只有初始幀中的目標，即難以提供大量的高質量數據去訓練一個性能良好的模型，所以深度學習方法應用在實時目標有部分缺陷。而一些基于相關濾波的跟蹤算法，處理速度可以達到數百幀每秒，可以廣泛應用于實時跟蹤系統中，如核相關濾波法(Kernel Corrolation Filter，KCF)[19]、多功能自適應核相關濾波[20]、目標跟蹤評估算法[21]等。

在以前的文章中，已經探索了許多基于視覺的無人機目標跟蹤系統。文獻[4]等提出了一種基于顏色過濾算法和形狀過濾算法的四軸飛行器目標跟蹤控制系統，可以根據被跟蹤目標的顏色和形狀特征控制飛行器執行跟蹤任務。文獻[5]等提出一種基于PID和模糊控制算法結合的四軸飛行器運動控制方法，可根據opencv識別和跟蹤目標顏色，并在AR.Drone.2.0四軸飛行器上驗證了結果。文獻[6]提出了兩種基于DSST和KCF的跟蹤算法，使用無模型、比例自適應的方式來估計與被跟蹤目標的距離。在用于四軸飛行器跟蹤任務時，可以選擇人作為跟蹤目標，而不再根據顏色或者形狀進行區分。這些研究的共同點是，由于四軸飛行器機載的處理系統效率有限，他們都是接收單目攝像頭傳回的視頻流，并在地面站進行圖像處理。其在視頻流處理過程時采用逐幀處理的方式，而在執行跟蹤任務時，對跟蹤結果影響不大的圖片幀不必要占用處理時間，所以這種方式浪費了大量的計算資源。故文中提出一種基于注意力模型的圖片幀預處理算法，針對單目攝像頭采集的視頻圖像信息，使用注意力模型動態地過濾到部分對跟蹤結果影響不大的部分幀，通過反饋控制及時根據誤差估計調整四軸飛行器姿態，旨在實現四軸飛行器高實時性目標檢測與跟蹤。

1 四軸飛行器動力學模型

四軸飛行器機架呈交叉狀，有4個軸臂，分別安裝有4個螺旋槳和電機。從結構上，四軸飛行器可以分為“十”形和“×”形兩種機型。從本質上講這兩種機型無明顯區別，筆者主要針對“×”形進行研究。

在對飛行器進行姿態估計時，一般會用到機體坐標系和本地坐標系，如圖1所示。機體坐標系原點在四軸飛行器重心位置，x軸為四軸飛行器前進方向，z軸垂直x軸向下，y軸右手定則確定。本地坐標系能夠確定無人機相對于地面的位置，是以飛行器的初始位置為原點，在水平面正北方向為x軸正方向，正東方向為y軸正方向，垂直地面向下為z軸正方向。規定四軸飛行器繞z、y、x軸旋轉分別得到偏航角y(ψ)、俯仰角p(θ)、滾動角r(φ)。通過機體坐標系向本地坐標系的坐標變換，可以將數據運用到各項計算，其轉換矩陣如下：

圖1 四軸飛行器坐標系示意圖

(1)

如圖2所示，根據四軸飛行器結構的幾何關系對螺旋槳的拉力進行分析。設每個螺旋槳的拉力均為f，飛行器俯仰時螺旋槳拉力的變化量為Δfp，飛行器滾動時螺旋槳拉力的變化量為Δfr，飛行器偏航時螺旋槳拉力的變化量為Δfy。則可以得出每一個螺旋槳的拉力：

圖2 四軸飛行器拉力分解示意圖

(2)

(3)

(4)

(5)

四軸飛行器螺旋槳的轉速的變化可以改變螺旋槳的拉力，從而控制飛行器執行前進、后退、向左、向右等飛行動作。

2 基于特洛平臺的目標檢測與跟蹤系統設計與實現

在進行目標檢測與跟蹤時，系統終端處理圖片幀的實時性和飛行器控制的穩定性是需要重點考慮的兩個方面。在圖片幀預處理階段，傳統方法對飛行器傳回的圖片幀逐一處理，這樣在準確性上確實效果較佳，但消耗大量時間。針對飛行器實時性要遠大于準確性的要求，文中提出一種基于注意力模型的圖片幀預處理方法，選出對目標跟蹤處理貢獻度較高的部分幀，配合基于像素點的反饋控制算法，旨在提高飛行器目標檢測與跟蹤系統的實時性和飛行器控制的穩定性。

目標檢測與跟蹤系統結構如圖3所示，系統運行流程設計如下：

圖3 系統結構圖

(1)系統初始化。系統終端與四軸飛行器建立連接，并通過用戶數據報協議協議(User Datagram Protocol，UDP)控制飛行器起飛，同時系統終端接收實時傳回的視頻流并按視頻幀的方式讀取。

(2)計算選框位置偏差。將圖片幀送入目標檢測與跟蹤模型，得到被跟蹤目標選框在視頻幀中位置偏差。

(3)注意力模型。對后續幀運用注意力模型挑選貢獻度較高的部分幀進行處理。

(4)飛行器反饋控制。根據反饋控制將選框位置偏差值轉換為相應的四軸飛行器電機轉速變化。

(5)飛行器姿態調整。將變化后的電機轉速值發送給無人機，使其及時作出姿態調整。

2.1 基于注意力模型的圖片幀處理

2.1.1 注意力模型

對四軸飛行器傳回的視頻流，如果每一幀都送入目標檢測和跟蹤算法，則會消耗大量的資源，進而降低四軸飛行器跟蹤的實時性。若先對圖片幀進行判別，則選擇對跟蹤結果貢獻度較大，即對結果的影響較高的某些幀，這樣動態地選取部分圖片幀進行處理，可有效縮短四軸飛行器采集圖片幀后預處理時間。

注意力模型又稱為注意力機制。在計算能力有限的情況下，注意力模型作為一種資源分配方案，將有限的計算資源用來處理更重要的信息，因此將注意力模型的思想運用到對視頻幀的預處理中。首先，在所有的輸入信息上計算注意力分布；然后，再根據注意力分布來計算輸入信息的加權平均。注意力模型的核心在于計算注意力分布，用打分函數來計算每個輸入向量和查詢向量之間的相關性，其中查詢向量是一個和任務相關的表示。給定一個和任務相關的查詢向量q，注意力變量z∈[1，N]表示被選擇信息的索引位置，X=[x1，…，xN]∈RD×N表示N組輸入信息。則計算在給定q和X下，選擇第i個輸入向量的概率αn：

(6)

其中，αn稱為注意力分布，s(x，q)為注意力打分函數。

2.1.2 運用注意力模型的圖片幀預處理

如圖4所示，基于注意力機制的原理，當開始目標跟蹤任務后，得到第1幀圖片的分析結果時，則會對該圖片幀進行打分，從而決策在后續的第i+1幀繼續處理(i為s對應結果)。

圖4 注意力模型的圖片幀預處理

2.2 基于反饋控制算法的四軸飛行器控制

四軸飛行器傳回的圖片幀，經過注意力模型的預處理后再次送入YOLO+KCF模型；如果跟蹤目標發生位移，則在圖片幀上會顯示為選框位置和面積的變化，因此可將飛行器飛行控制簡化為采用基于PID控制的圖片中心位置像素點偏差自適應反饋控制和基于像素面積偏差反饋的四軸飛行器位置控制，分別控制四軸飛行器的左右偏航和前后位移。該四軸飛行器控制算法情景設計如下。

情景1當選框中心點相對圖片幀中心點左偏移時，超過設定閾值，表明目標即將從左側逃離跟蹤。此時，飛行器應向左偏航。

情景2當選框中心點相對圖片幀中心點右偏移時，超過設定閾值，表明目標即將從右側逃離跟蹤。此時，飛行器應向右偏航。

情景3當選框像素面積變大時，超過設定閾值，表明目標與飛行器距離較近。此時，飛行器應向后退。

情景4當選框像素面積變小時，超過設定閾值，表明目標與飛行器距離較遠。此時，飛行器應向前進。

2.2.1 基于PID控制的圖片中心位置像素點偏差自適應反饋控制

PID[22]控制目前在工業控制系統中應用較廣泛，它是一個典型的單位負反饋系統，系統由PID控制器和被控對象組成。其中PID分別為比例、積分、微分(Proportional、Integral、Differential)。PID控制的核心就是計算出確定的給定值與實際的輸出值之間的偏差e(t)，然后根據比例—積分—微分的線性組合關系計算出控制量u(t)，再通過改變控制量來控制被控對象。PID控制可連續地表示為

(7)

但在實際應用中，計算機采樣無法做到真正的連續，所以將PID進行離散化處理，分為增量式PID和位置式PID。增量式PID控制用數學表示為

u(t)=u(t)+Kp[e(k)-e(k-1)]+Kie(k)+Kd[e(k)-2e(k-1)+e(k-2)] 。

(8)

采用增量式PID對飛行器電機轉速進行閉環控制，通過計算被跟蹤目標的選框像素中心點與當前圖片幀像素中心點的水平偏差e(k)，并將該偏差作為誤差參數，通過PID控制計算公式得到四軸飛行器此刻的偏航轉速u(t)為speed；使用send_rc_control(0，fb，0，speed)函數向四軸飛行器發送指令，控制飛行器左右偏航。

在四軸飛行器執行目標跟蹤任務時，PID參數初始化為[0.4，0.4，0.1]；e(k)、e(k-1)和e(k-2)分別代表本次誤差、上次誤差和上上次誤差，誤差初始值默認為0，偏航控制系統工作流程如圖5所示。如圖5(a)所示，目標的中心與圖片幀中心有偏差，計算此數值e(k)作為輸入參數，通過增量PID公式計算出speed，使無人機向右偏轉，同時更新誤差e(k-1)=e(k)。

圖5 偏航控制系統工作流程

2.2.2 基于像素面積偏差反饋的四軸飛行器位置控制算法

在執行目標跟蹤任務時，需要使四軸飛行器和被跟蹤目標之間保持安全距離。對距離的判斷采用一種基于選框內像素點所占面積的方法。首先設定一個面積區間A[a，b]。當被跟蹤目標遠離四軸飛行器并且選框面積大于b時，飛行器會向前移動；當被跟蹤目標靠近四軸飛行器并且面積小于a時，飛行器會向后移動，文中使用的面積區間A[60 000，68 000]，從四軸飛行器傳回的視頻流經cv2.resize()函數處理，統一分辨率為960×720在終端顯示，前后位移控制如圖6所示。計算選框面積時，使用數組boundingbox[x，y，w，h]接收KCF跟蹤器在當前圖片幀的跟蹤結果。(x，y)表示選框的左上角坐標，w、h分別代表選框的長、寬，則選框的面積為A=w×h。

圖6 位移控制

3 實驗結果與分析

3.1 軟硬件平臺介紹

四軸飛行器目標檢測與跟蹤研究在筆者電腦中進行，電腦配置Intel(R)Core(TM)i5-7200U處理器，NVIDIA GeForce 940MX顯卡。開發環境采用python 3.7、CUDA 10.0、cuDNN 7.6、OpenCV 3.4。使用的硬件平臺是四軸飛行器Tello，如圖7所示。Tello是深圳睿熾科技生產的小型航拍無人機，結合了DJI大疆飛控技術和英特爾芯片，具備紅外定高、氣壓計定高、下視視覺等功能，支持一鍵拋飛、全向翻滾，并且攜帶500萬像素攝像頭，可以進行720P高清圖傳。

圖7 四軸飛行器Tello

3.2 系統框架(YOLOV4-tiny + KCF)介紹

YOLO[15](You Only Look Once)是一種基于深度神經網絡的對象識別和定位算法。其采用端到端的設計思想，將目標定位和目標識別這兩個階段合二為一，對同一圖片只需掃描一次。該算法目標識別的速度可以達到實時檢測的要求。基于對四旋翼無人機目標檢測的實時性考慮，飛行器回傳視頻圖像的目標識別處理對速度的需求大于識別精度的需求。文中使用的是YOLOV4-tiny版本，其于2020年6月正式發布，是YOLOV4的輕量型版本，具有更快的識別速度。

KCF[16]是一類基于模板匹配的可視物體跟蹤方法，它把標識目標的矩形看作一個模板，用于搜尋物體在下一個視圖中的位置，其在計算模板時采用基于傅里葉變換的計算方法，顯著提高了在下一幀中識別匹配窗口的速度。

采用YOLOV4-tiny和KCF結合的方式，實現四軸飛行器目標檢測與跟蹤，基于KCF的跟蹤算法可以在傳回的圖片幀中框選跟蹤目標(即使該目標沒有被YOLOV4-tiny目標檢測算法精準識別)。此處理方式，極大可能地彌補了YOLOV4-tiny目標檢測算法因輕量化導致的識別度略低的缺陷。結合飛行器的YOLO+KCF算法實現如圖8所示，通過加載數據文件、權重文件以及配置文件利用GPU對傳入的圖片幀進行多次卷積和池化處理，隨后經過全連接層和輸出層。最后將經過目標檢測處理的圖片幀加載KCF跟蹤器，并將最終結果顯示在系統終端。

圖8 圖片幀通過YOLO+KCF算法

3.3 實驗結果與分析

為了驗證筆者提出的基于注意力模型的圖像預處理，以及基于反饋控制的無人機姿態調整，布置如下實驗環境。

3.3.1 目標檢測測試

為了減少因無人機執行目標檢測任務時飛行速度帶來的精度下降，設計如下對比實驗。因為不同的打桿桿量對應無人機在6個自由度的不同速度，分別選取40%、60%、80%的打桿桿量(對應3組不同飛行速度實驗)，因此可以根據檢測模型在無人機不同打桿桿量下進行實驗，確定小型四軸飛行器對目標檢測的最佳速度。

四軸飛行器起飛后進入目標檢測階段，其飛行實景以及攝像頭采集到的圖片經過處理后的結果如圖9、10、11所示，處理后的結果會在系統終端顯示，其中多個目標已經被矩形框框住，并在矩形框的上方標記類別。圖片幀左上角顯示檢測出的類別及其概率，左下角為此時四軸飛行器電池電量和姿態角。分析實驗結果：當桿量設置為40%時，飛行器在偏航測試中目標識別率約為97%，在位移測試中目標識別率約為95%，都具有較好的識別精度，但該狀態下飛行器姿態調整速度較慢，會影響其執行任務的實時性；當桿量設置為60%時，飛行器在偏航測試中目標識別率約為94%，在位移測試中目標識別率約為95%，具有較好的識別精度，同時飛行器也能及時完成姿態調整；當桿量設置為80%時，飛行器在位移測試中目標識別率約為90%，但在偏航時因飛行器偏航速度較快，目標識別率僅約為80%。綜上實驗結果，在桿量設置為60%時，對目標識別的精度和飛行器姿態調整的速度都有較好的效果，因此設置桿量60%進行后續目標檢測與跟蹤控制實驗。

圖9 打桿桿量設置為40%(實驗1)

圖10 打桿桿量設置為60%(實驗2)

3.3.2 目標跟蹤與控制測試

為了驗證目標跟蹤的實時性和穩定性，設計實驗場景：工作人員從辦公室內的辦公桌旁出發，打開門，途經走廊到達會議室，飛行器實時調整姿態角和前后距離跟蹤工作人員飛行。

四軸飛行器起飛后，先進入目標檢測階段，其拍攝到的圖片以及經過處理后的圖片如圖12所示。其中圖12(a)為飛行器在執行飛行實驗時的實景；圖12(b)為系統終端顯示的目標檢測結果，圖片幀中多個目標已經被矩形框框住，并在矩形框的上方標記類別。

在四軸飛行器飛行狀態下，自定義選擇目標，如圖13所示，圖13(a)為飛行器與被跟蹤目標的初始位置，圖13(b)為在系統終端選中被跟蹤目標后的顯示結果。可以看出，與前面目標檢測不同的是：目標跟蹤時，圖片幀右上角實時顯示此刻飛行器應做出的飛行調整；左下角顯示了此刻選框的中心點坐標和選框的面積，便于調整反饋控制時的參數。隨后，被跟蹤目標發生位移，發生位移時飛行器飛行實景和系統終端的處理結果，如圖14所示。

圖13 目標跟蹤初始狀態

圖14 四軸飛行器目標跟蹤

分析四軸飛行器實驗中的圖片資料，位移1、位移2和位移8被跟蹤目標正面對著飛行器，其他位移狀態被跟蹤目標背面對著飛行器，并且伴隨著被跟蹤目標的不斷移動。目標跟蹤算法能夠在跟蹤過程中及時調整選框位置和選框大小，飛行器能夠及時作出姿態調整。飛行過程中部分關鍵點姿態角變化如圖15所示，圖中標記出幾處拐點，表示此時飛行器正在調整偏航角使得飛行器正對被跟蹤目標，其偏航角調整角度在[-60°，70°]區間內并未發生反復左右震蕩偏航現象。

圖15 姿態角變化

圖16中圓形連接的折線為被跟蹤目標運動軌跡，三角形連接的折線為四軸飛行器飛行軌跡。根據軌跡點所示，確定被跟蹤目標后，四軸飛行器向目標飛行，調整與目標之間的距離。在目標移動時，其在經過沙發和左側門兩處時距離偏差較大。其中，在沙發處飛行器和被跟蹤目標距離較遠，導致飛行器跟蹤目標過門時出現飛行器未飛出辦公室的情況，經目標緩慢移動調整與飛行器距離后，飛行器才成功過門。出現這種情況，一方面是因為飛行器目標跟蹤算法的精度問題，在處理過大量視頻幀后，選框的大小判斷出現偏差，導致飛行器與目標距離誤差較大；另一方面是由于像素面積偏差反饋的四軸飛行器位置控制算法中面積參數范圍較大。

為了量化圖16中飛行器軌跡與被跟蹤目標運動軌跡之間的偏離，考慮到飛行器在執行跟蹤任務時，被跟蹤目標移動距離未超過設定的飛行器有效跟蹤面積區間，此時不必調整飛行器距離只需適當調整偏航角。因此，采用距離偏差來評價各組采樣點(t時刻飛行器位置與被跟蹤目標位置)的偏離程度，用方差來評價所有采樣點的整體偏離情況。通過測量實驗中各組采樣點的距離，可得飛行器與被跟蹤目標距離偏差最大值約為35.3 cm，最小值近似為0 cm(由于采用的位移跟蹤控制是一個面積區間，因此實驗時大部分樣本點都在此區間內)，平均值約為3.33 cm。通過測量的距離偏差，計算得所有采樣點的方差值為69.15。實驗結果表明，飛行器飛行軌跡與被跟蹤目標運動軌跡之間偏差較小，執行跟蹤任務時大部分軌跡點處于設定的安全跟蹤距離范圍內。因此，筆者提出的算法在目標跟蹤的實時性、穩定性上具有較好的效果。

圖16 飛行軌跡與被跟蹤目標軌跡

為了進一步驗證算法實時性，在實驗過程中，隨機抽取20 s的視頻流。因飛行器每秒傳輸30幀圖片，則20 s共傳輸圖片600幀，利用文中提出的注意力模型進行圖片預處理后，隨機抽取的600幀中只處理了23幀，處理時間約為0.327 5 s。若未使用注意力模型，每1幀都處理，共需要約8.772 s。在實際飛行過程中，隨著圖片幀的增多，預處理階段帶來的效果是很明顯的，圖17為系統處理一幀圖片所需要的時間。表1為在實驗環境下，基于注意力模型的圖片幀預處理算法與其他目標檢測跟蹤算法在接收到相同圖片幀時，處理圖片的時間對比。表2為文中模型隨著接收幀數的增加，所處理的圖片幀數量的變化。

表1 接收50個圖片幀處理效率對比

表2 文中模型處理圖片幀效率

圖17 圖片幀處理時間

可見，基于注意力模型的圖像預處理可有效節省圖像幀處理時間，節約大量計算資源，從而提升了四軸飛行器的目標跟蹤實時性，為四軸飛行器的實時跟蹤飛行應用提供了保障。

4 結束語

基于四軸飛行器單目攝像頭采集到的視覺信息，將視頻流分解為圖片幀。針對圖片幀處理速度和四軸飛行器姿態控制兩方面，分別提出基于注意力模型的圖片預處理和基于反饋控制的四軸飛行器姿態調整。實驗結果表明，文中提出的方法在執行目標監測與跟蹤任務時，具有較好的實時性和穩定性效果。利用注意力模型的圖片預處理，可以合理地舍棄部分對跟蹤結果影響不大的圖片幀，節省大量計算資源；基于反饋控制的四軸飛行器姿態調整，使四軸飛行器能夠及時作出偏航和進退調整。下一步將考慮進一步結合深度學習框架，優化目標跟蹤功能，使四軸飛行器能自主選擇跟蹤目標，并完成跟蹤任務。