05.25 四旋翼飛行器追蹤地面移動目標控制策略研究

0 引言

四旋翼飛行器對地面移動目標的追蹤在軍用、民用及商業化應用中發揮著越來越重要的作用^[1]，對敵軍車輛、坦克實時跟蹤並偵查，以獲取敵軍行蹤；對災後的人員搜救，以獲取實時災情；對快速移動的物體進行拍攝，以獲取可靠的影視資料等，均涉及飛行器對地面移動目標的追蹤。李永健等人給出了採用OpenCV處理圖像獲取反饋信號的方案^[2]，但OpenCV運行需機載計算機或採用上位機，不適合於小型嵌入式系統；文獻[3]給出了採用光流法對運動目標進行檢測的方案，但由於光流法更多地是檢測移動目標的速度，無法檢測地面移動目標相對於飛行器的位置，追蹤效果不理想；文獻[4]給出了採用ADAMS與MATLAB仿真的多回路PID位置跟蹤控制的模型，但該模型的實現較為困難，需運行速度較快的處理器。文獻[5]給出了雙閉環軌跡跟蹤與控制的方案，提高了飛行器追蹤地面移動目標的魯棒性；文獻[6]採用OpenMV製作了全自動模擬目標搜救系統，為飛行器追蹤地面移動目標提供了新方案。

要完成飛行器對地面移動目標的快速追蹤，需滿足以下要求：首先，飛行器本身運動控制須非常靈活，響應迅速，滯後少；接著，要為控制系統提供必要的反饋，主要是運動目標與飛行器的位置偏差以及運動目標的移動速度；最後，需根據反饋對飛行器做出可靠的控制。

為了實現對地面快速移動目標的實時追蹤，飛行器的響應速度必須較快，當地面目標移動時，飛行器應當能夠根據地面目標的位置變化實時追蹤。如果飛行器的響應速度較慢，當地面目標移動速度較快時，飛行器未能實時追蹤，地面目標可能已經偏離攝像頭視野，失去反饋信號，繼而失控。主要從以下兩方面提高飛行器打舵響應速度。

1.1 姿態PID控制的改進

通常，飛行器姿態控制為串級PID控制^[7]，控制流如圖1所示。

飛行器在懸停狀態下，PITCH、ROLL的姿態角都應當為0，當飛行器做出追蹤地面移動目標的動作時，PITCH、ROLL姿態角應當轉到一定的角度，才可完成追蹤，判斷飛行器響應速度的標準也就為當飛行器從0°姿態角轉到一定角度姿態角的時間，如果這個過程時間越短，則飛行器的響應速度就越快，反之越慢。為了減少該過度過程的時間，提高響應速度，在外環角度PID控制器上做出了改進，外環角度PID控制器的表達式為^[8](1):

其中，u(t)為輸出的角速度，K_p為比例係數，K_i為積分系數，K_d為微分系數，e(t)為期望的姿態角和實際的姿態角的偏差。式(1)中，微分項代表姿態角偏差變化的速率，當期望的姿態角為零時，姿態角偏差變化的速率也就是姿態角變化的速率，姿態角變化的速率為角速度，而陀螺儀輸出的原始數據即為角速度。因此，可用陀螺儀的原始數據代替外環角度PID控制器的微分項，如式(2)所示：

其中，Gyro為陀螺儀的原始數據，Gyro_x代表著ROLL方向的角速度，Gyro_y代表PITCH方向的角速度。陀螺儀輸出的原始數據動態響應性能好，能實時地反映飛行器的角速度。相比採用角度偏差的微分作為微分項，陀螺儀輸出的角速度作為微分項更能體現飛行器姿態角的變化趨勢，形成有效的震盪阻尼，進而抑制飛行器的震盪。

1.2 參數的整定

整定合適的PID參數，可提高飛行器的動態響應性，串級PID中，內環為角速度控制，在角速度控制環中，期望的角速度為外環輸入的角速度，實際的角速度為陀螺儀輸出的角速度數據，輸出的是PWM脈寬直接控制電機。因此該控制環既控制著角速度又直接控制電機，是最基礎、最重要的控制環，內環應當根據角速度偏差輸出電機轉速及時修正角速度，能否及時修正角速度偏差由該控制環決定，飛行器的動態響應性能也由該控制環決定。在整定該控制環參數過程中，可適當增大K

當給內環一個期望的角速度時，內環角速度變化數據如圖3所示。

外環則為角度控制環，在角度控制環中，輸入的期望值為視覺追蹤所期望的姿態角，輸入的實際值為姿態解算的實際姿態角，輸出的量作為角速度輸入給內環。在角度控制環中，外環角度的變化通過內環角速度的變化去改變，即外環負責根據姿態角的偏差計算出應當以多大的角速度去修正該姿態角偏差，內環負責根據角速度的偏差計算出應當以多大的電機轉速去修正該角速度偏差。外環主要控制對象為姿態角，而姿態角改變是視覺追蹤的基礎，因此，外環決定了飛行器能夠追蹤移動速度多快的目標。在內環的基礎上，外環參數整定做到動態響應即可，先增大K_p值，當飛行器能夠迅速修正姿態角偏差時，增大K_d抑制震盪；當姿態角存在靜態誤差時，增大K_i修正靜態誤差^[10]。整定參數K_p=0.8、K_i=0.05、K_d=0.6後，打舵測試如圖4所示。

如圖4所示，當期望的姿態角發生改變時，能以較大的角速度去修正姿態角偏差，迅速響應，過程過渡時間短，系統基本無震盪，達到了較為理想的狀態。

2 移動目標位置獲取

擁有反饋是一個系統閉環控制的重要條件，在飛行器追蹤小車控制系統中，飛行器要獲取小車的位置最簡單有效的方式為判斷小車在攝像頭視野範圍的位置。期望的位置為(0，0)，當小車在攝像頭視野範圍中心時，小車在攝像頭視野範圍的位置為（0，0）；若小車不在中心位置，則期望位置與實際位置存在偏差，需通過PID控制修正該偏差，以此達到追蹤小車的效果。

2.1 OpenMV簡介

Python作為腳本語言，無法進行底層的操作，而MicroPython是一個Python3的語言解析器，非常小巧，只需要256 KB的代碼空間及16 KB RAM空間，並針對微處理器受限的環境進行了優化，藉助MicroPython，32位的ARM處理器(比如STM32F405)就可以採用Python進行底層操作。OpenMV是一款基於STM32F765和OV7725攝像頭且安裝了MicroPyhon解析器的開源機器視覺模塊，其底層驅動和圖像處理庫用C語言進行封裝，並參考了部分OpenCV庫，預留接口供MicroPython調用。採用OpenMV進行開發，可通過Python腳本藉助MicroPython調用底層驅動與圖像處理庫，可節省開發週期，同時封裝好的驅動層與圖像處理庫降低了嵌入式與圖像處理開發的門檻

2.2 OpenMV獲取小車位置的方法

在OpenMV內置的圖像處理庫中，包含了尋找色塊的庫函數find_blobs，該函數對於彩色圖像而言，輸入LAB參數值，可輸出找到的各個彩色色塊中心點在攝像頭視野範圍的位置；對於灰度圖像而言，輸入的是灰度閾值，可輸出找到的各個灰色色塊中心點在攝像頭視野範圍的位置。當處理彩色圖像時，確定LAB參數值是關鍵，若參數選擇不當，則可能無法識別目標色塊，在實際的參數確定過程中，需多次試驗，確定目標色塊的LAB參數範圍。圖5為確定綠色LAB參數值的一次仿真，在次仿真中，L的範圍為35～65，A的範圍為-60～-25，B的範圍為0～35。在該仿真中，並無噪聲影響到對綠色色塊的識別，是一個較為理想的參數，圖6為在實驗過程中，由該LAB參數對綠色色塊的識別。

當處理灰度圖像時，由於灰度圖像每一個像素點的顏色只需要用灰度值來區分，其中，每一個像素點的取值範圍為0～255，當小於設定閾值時，判定為黑色；反之，判定為白色，調整該閾值即可區分黑色色塊與周圍環境。

由於LAB色彩模型基於人對顏色的感覺，L代表亮度，A代表從洋紅到綠色的範圍，B代表從黃色至藍色的範圍[13]，因此，該色彩模型適合於人眼容易區分的色彩，不適合對黑色色塊的捕捉。但實際環境是複雜的，當需要捕捉黑色色塊時，採用灰度圖像，可提高捕捉色塊的魯棒性。彩色圖像存儲維度比灰度圖像大，因此處理速度也較慢，但實驗測試發現，使用OpenMV3硬件平臺，採用QQVGA圖像，find_blobs函數處理灰度圖像可達到約60幀/s，處理彩色圖像可達到約40幀/s，均能達到實時控制的需求。因此，可根據實際情況採用灰度圖像或彩色圖像，具體處理流程圖如圖7所示,計算色塊座標的方法如圖8所示。

如圖8所示，由OpenMV捕捉到小車位於圖像第n行、第m列個像素點之後，可計算出小車的座標(x，y），這就是系統的反饋數據。得到該數據後，將該數據發送至飛控，在追蹤小車過程中，期望的小車的位置始終是（0，0）。

3 追蹤控制實現

在獲取到系統反饋的基礎上，可根據反饋信號對系統進行控制，主要是修正期望位置與實際位置的偏差，即可實現對小車的跟蹤。而PID控制是基於修正偏差的控制，能夠有效修正偏差，但作為一個追蹤的控制過程，還需要考慮到追蹤的速度。如果小車速度較快，僅僅考慮到位置控制則可能追不上，因此，本文還設計了位置-速度的雙環PID控制。

3.1 單環控制的實現

若僅考慮到小車位置變化，飛行器及時調整姿態角，從而改變飛行器的位置，可達到追蹤目的，具體控制流程圖如圖9所示。

採用單環位置控制時，調整PID參數K_p=0.6、K_i=0.16、K_d=0.43時，飛行器移動座標的X軸、Y軸如圖10、圖11所示，在小車靜止或移動速度較慢的情況下，飛行器能夠懸停在小車上方，可對移動速度較慢的小車進行追蹤。

在圖10中小車座標X軸數據在-15左右上下波動，在圖11中座標Y軸數據在15左右上下波動，而不是在理想的0值上下波動，存在靜態誤差。形成該靜態誤差的主要原因是加速計數據存在較大誤差。加速計每次上電後的誤差都有一定的變化，隨時間偏移變化較小^[14]，而飛行器無法做到每次上電時刻都處於水平狀態，加速計Z軸數據不一定等於重力加速度，因此無法上電校準，形成了誤差，飛行器此後以該不準確的數據解算姿態，形成姿態的誤差^[15]，造成飛行器位置飄動，位置PID控制器在克服由於姿態誤差造成的飛行器的位置飄動時，形成了位置控制時的靜態誤差。

3.2 雙環控制的實現

為了解決在單環控制時小車速度較快、飛行器無法及時追蹤的問題，設計了雙環控制，控制流程圖如圖12所示。

該控制將小車的位置與小車的移動速度作為反饋數據，可有效修正位置偏差的同時修正速度偏差。與單環PID控制相比，雙環PID控制可追蹤速度更快的小車，並且位置座標浮動也比單環控制的小，X軸與Y軸的位置數據變化如圖13、圖14所示。

將圖10與圖13進行比較：圖10中縱座標數據上下飄動範圍為35左右，圖13中縱座標數據上下飄動範圍為20左右，說明在X座標軸上，串級PID控制使得飛行器飄動比單級PID控制小，串級PID控制比單級PID控制更加穩定。同理，將圖11與圖14進行比較：圖11中縱座標數據上下飄動範圍為30左右，圖14中縱座標數據上下飄動範圍為10左右，說明在Y座標軸上，串級PID使得飛行器飄動比單級PID控制小，串級PID控制比單級PID控制更加穩定。在位置-速度PID控制中，位置PID控制器輸出至速度PID控制器，當期望位置與當前位置偏差越大時，輸出的期望速度越大，速度PID控制器輸出的期望姿態角也越大，飛行器會以更快的速度飛至小車上方，以完成追蹤速度較快的小車的任務。綜上所述，位置-速度PID控制器不僅提升了飛行器懸停的穩定性，還使得飛行器能夠追蹤速度較快的小車。綜上，在該飛行器控制系統中，從姿態的串級PID控制，到位置的串級PID控制，構成了一個4環PID控制系統。控制流程如圖15所示。

4 實驗驗證

本次實驗採用S500機架，STM32F407處理器作為飛行控制板，整定響應迅速的PID參數，採用OpenMV獲取飛行器相對於小車的位置數據，採用串級PID控制飛行器相對於小車的位置。測試飛行器在靜止目標上的懸停以及在運動上的跟蹤，並測試包含灰度與彩色兩種類型的圖像。測試步驟如圖16所示。

在圖16所示的實驗步驟中，先將小車放置在區域8，然後飛行器在區域10起飛，起飛之後，OpenMV採用灰度圖像捕捉大黑點，並在大黑點上方懸停，懸停10 s之後，飛行器往區域8方向飛。同時OpenMV採用彩色圖像捕捉綠色小車，若未能捕捉到小車，則一直往前飛，直到捕捉到綠色小車為止，飛行器成功懸停在綠色小車上方10 s之後，小車開始在區域1至區域9各個區域移動，飛行器追蹤小車，並在區域1至區域9各個區域上方飛行。實驗證明，飛行器可有效在大黑點與綠色小車上方懸停，並能成功追蹤綠色小車。

5 結論

本文設計了一種四旋翼飛行器追蹤地面移動目標的控制策略，並採用該策略成功實現了飛行器追蹤地面移動目標。改良了四旋翼飛行器的PID控制算法，整定了合適的PID參數，成功提高了四旋翼飛行器的打舵響應速度。採用OpenMV獲取地面移動目標的位置，並採用串級PID控制器修正期望位置與當前位置的偏差，成功實現了四旋翼飛行器追蹤地面移動目標。相比單環PID控制飛行器位置，串級PID控制不僅提高了懸停的魯棒性，還使得飛行器可追蹤移動速度較快的目標。

參考文獻

[1] 劉添勝，陳益平，陳凱旋.四旋翼飛行器在臺山輸電巡維的應用[J].電工技術,2014(2)：65-66.

[2] 李永健.基於機器視覺的四旋冀無人機定點著陸系統設計與實現[D].廣州：華南理工大學，2015.

[3] 王卉，李麗霞，劉慧潔，等.基於四旋翼飛行器的光流法動態目標檢測[J].瀋陽工程學院學報(自然科學版)，2017，13(1)：76-81.

[4] 姜香菊，曾幼涵，劉二林.四旋翼飛行器帶速度補償的多回路PID位置跟蹤控制[J].測控技術，2017，36(4)：74-78，84.

[5] 許璟，蔡晨曉，李勇奇，等.小型四旋翼無人機雙閉環軌跡跟蹤與控制[J].控制理論與應用，2015，32(10)：1335-1342.

[6] 潘麗靜，張虹波，周婷婷.全自動模擬目標搜救系統的設計與實現[J].電腦知識與技術，2016，12(28)：178-180.

[7] 賈峰，孫曼，秦磊.基於STM32的四旋翼飛行姿態串級控制[J].科學技術與工程，2015，15(6)：220-225.

[8] 李俊，李運堂.四旋翼飛行器的動力學建模及PID控制[J].遼寧工程技術大學學報(自然科學版)，2012，31(1)：114-117.

[9] 段棟棟.高精度MEMS陀螺儀的濾波算法研究[D].成都：電子科技大學，2014.

[10] 馮慶端，裴海龍.串級PID控制在無人機姿態控制的應用[J].微計算機信息，2009，25(22)：9-10，45.

[11] Pan Lijing，Zhang Hongbo，Zhou Tingting，et al.The design and implementation of automatic simulation target search and rescue system[J].Computer Knowledge & Technology，2016，12(10)：178-180.

[12] ISAKOV A.Using open MVL shell in research and education[C].Manufacturing Modelling，Management，and Control，2013：2191-2196.

[13] 張宏建.Lab色彩模式在圖像處理中的應用[J].福建電腦，2011，27(1)：146-147.

[14] 鄭海潮.四軸飛行器自主飛行控制及避障系統研究[D].成都：電子科技大學，2016.

[15] 冀亮,錢正洪,白茹.基於四元數的四軸無人機姿態的估計和控制[J].現代電子技術，2015(11)：112-116.

作者信息:

黃瑞敏1，2，李建奇1，2，張 斌1，楊民生1，王文虎1

（1.湖南文理學院 電氣與信息工程學院，湖南 常德415000；

2.湖南文理學院 洞庭湖生態經濟區建設與發展湖南省協同創新中心，湖南 常德415000）

閱讀更多 電子技術應用 的文章

關鍵字: 體育 湖南 無人機