0 引言

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

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

1 系統(tǒng)響應速度提高

    為了實現(xiàn)對地面快速移動目標的實時追蹤，飛行器的響應速度必須較快，當?shù)孛婺繕艘苿訒r，飛行器應當能夠根據(jù)地面目標的位置變化實時追蹤。如果飛行器的響應速度較慢，當?shù)孛婺繕艘苿铀俣容^快時，飛行器未能實時追蹤，地面目標可能已經(jīng)偏離攝像頭視野，失去反饋信號，繼而失控。主要從以下兩方面提高飛行器打舵響應速度。

1.1 姿態(tài)PID控制的改進

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

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

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

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

1.2 參數(shù)的整定

    整定合適的PID參數(shù)，可提高飛行器的動態(tài)響應性，串級PID中，內(nèi)環(huán)為角速度控制，在角速度控制環(huán)中，期望的角速度為外環(huán)輸入的角速度，實際的角速度為陀螺儀輸出的角速度數(shù)據(jù)，輸出的是PWM脈寬直接控制電機。因此該控制環(huán)既控制著角速度又直接控制電機，是最基礎、最重要的控制環(huán)，內(nèi)環(huán)應當根據(jù)角速度偏差輸出電機轉速及時修正角速度，能否及時修正角速度偏差由該控制環(huán)決定，飛行器的動態(tài)響應性能也由該控制環(huán)決定。在整定該控制環(huán)參數(shù)過程中，可適當增大K_p值，當系統(tǒng)產(chǎn)生等幅震蕩時，加入K_d；當加入K_d能抑制系統(tǒng)的等幅震蕩時，繼續(xù)加入K_p，直到加入K_d不能抑制系統(tǒng)的等幅震蕩為止。采取此臨界狀態(tài)的K_p作為內(nèi)環(huán)的比例系數(shù)，采取能抑制住震蕩的K_d作為內(nèi)環(huán)的微分系數(shù)。在抑制住系統(tǒng)的等幅震蕩之后，如果系統(tǒng)仍然以一定的角速度轉動,則適當加入積分項，以能消除系統(tǒng)的靜態(tài)誤差時的K_i作為微分系數(shù)。以繞陀螺儀y軸的角速度作為參考，在整定合適的參數(shù)K_p=1.5、K_i=0.5、K_d=1.6后，PITCH姿態(tài)角的角速度期望值為0時，繞陀螺儀y軸的角速度變化如圖2所示，考慮到陀螺儀數(shù)據(jù)會有零點漂移^[9]，以及受電機震動的影響，角速度變化數(shù)據(jù)在0上下±50°/s的波動是可以接受的。

    當給內(nèi)環(huán)一個期望的角速度時，內(nèi)環(huán)角速度變化數(shù)據(jù)如圖3所示。

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

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

2 移動目標位置獲取

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

2.1 OpenMV簡介

    Python作為腳本語言，無法進行底層的操作，而MicroPython是一個Python3的語言解析器，非常小巧，只需要256 KB的代碼空間及16 KB RAM空間，并針對微處理器受限的環(huán)境進行了優(yōu)化，借助MicroPython，32位的ARM處理器(比如STM32F405)就可以采用Python進行底層操作。OpenMV是一款基于STM32F765和OV7725攝像頭且安裝了MicroPyhon解析器的開源機器視覺模塊，其底層驅(qū)動和圖像處理庫用C語言進行封裝，并參考了部分OpenCV庫，預留接口供MicroPython調(diào)用。采用OpenMV進行開發(fā)，可通過Python腳本借助MicroPython調(diào)用底層驅(qū)動與圖像處理庫，可節(jié)省開發(fā)周期，同時封裝好的驅(qū)動層與圖像處理庫降低了嵌入式與圖像處理開發(fā)的門檻^[11-12]。

2.2 OpenMV獲取小車位置的方法

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

    當處理灰度圖像時，由于灰度圖像每一個像素點的顏色只需要用灰度值來區(qū)分，其中，每一個像素點的取值范圍為0～255，當小于設定閾值時，判定為黑色；反之，判定為白色，調(diào)整該閾值即可區(qū)分黑色色塊與周圍環(huán)境。

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

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

3 追蹤控制實現(xiàn)

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

3.1 單環(huán)控制的實現(xiàn)

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

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

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

3.2 雙環(huán)控制的實現(xiàn)

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

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

    將圖10與圖13進行比較：圖10中縱坐標數(shù)據(jù)上下飄動范圍為35左右，圖13中縱坐標數(shù)據(jù)上下飄動范圍為20左右，說明在X坐標軸上，串級PID控制使得飛行器飄動比單級PID控制小，串級PID控制比單級PID控制更加穩(wěn)定。同理，將圖11與圖14進行比較：圖11中縱坐標數(shù)據(jù)上下飄動范圍為30左右，圖14中縱坐標數(shù)據(jù)上下飄動范圍為10左右，說明在Y坐標軸上，串級PID使得飛行器飄動比單級PID控制小，串級PID控制比單級PID控制更加穩(wěn)定。在位置-速度PID控制中，位置PID控制器輸出至速度PID控制器，當期望位置與當前位置偏差越大時，輸出的期望速度越大，速度PID控制器輸出的期望姿態(tài)角也越大，飛行器會以更快的速度飛至小車上方，以完成追蹤速度較快的小車的任務。綜上所述，位置-速度PID控制器不僅提升了飛行器懸停的穩(wěn)定性，還使得飛行器能夠追蹤速度較快的小車。綜上，在該飛行器控制系統(tǒng)中，從姿態(tài)的串級PID控制，到位置的串級PID控制，構成了一個4環(huán)PID控制系統(tǒng)?？刂屏鞒倘鐖D15所示。

4 實驗驗證

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

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

5 結論

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

參考文獻

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

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

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

[4] 姜香菊，曾幼涵，劉二林.四旋翼飛行器帶速度補償?shù)亩嗷芈稰ID位置跟蹤控制[J].測控技術，2017，36(4)：74-78，84.

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

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

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

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

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

[10] 馮慶端，裴海龍.串級PID控制在無人機姿態(tài)控制的應用[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] 鄭海潮.四軸飛行器自主飛行控制及避障系統(tǒng)研究[D].成都：電子科技大學，2016.

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

作者信息:

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

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

2.湖南文理學院 洞庭湖生態(tài)經(jīng)濟區(qū)建設與發(fā)展湖南省協(xié)同創(chuàng)新中心，湖南 常德415000）