李文強　王嘉棟　隋國榮

摘? 要：無人機在室內等復雜環(huán)境中飛行時，存在GPS信號較弱、慣性傳感器累計誤差較大等問題，導致無法實現(xiàn)室內的精準定位。本文提出一種基于粒子群圓檢測算法的無人機目標定位方法，該方法通過OpenCV視覺模組進行圖像預處理，并通過增量式PID（Proportion Integration Differentiation）與圖像濾波相結合的粒子群圓檢測算法獲得目標標靶的核心坐標與半徑等參數(shù)，通過對運動的實時控制、增加判斷條件等提高無人機的標定效率與準確率。實驗結果表明，該方法能夠將無人機的位置調整時間控制在4 s以內，且將標定準確率提高到了90%，極大地縮短了無人機的標定時間，提升了無人機的位置標定準確率。

關鍵詞：粒子群圓檢測;增量式PID算法;圖像處理;目標定位;運動控制

中圖分類號：TP272? ? ?文獻標識碼：A

UAV Target Positioning Algorithm based on Particle Swarm Circle Detection

LI Wenqiang， WANG Jiadong， SUI Guorong

（College of Opto-electronic Information and Computer Engineering， University of Shanghai for Science and Technology， Shanghai 200093， China）

1935021414@st.usst.edu.cn; wangjd_0618@163.com; suigr@usst.edu.cn

Abstract： When flying indoors or in other complicated environments， UAVs （Unmanned Aerial Vehicle） usually have problems such as weak GPS signals and large cumulative errors of inertial sensors， which make it impossible to achieve accurate indoor positioning. This paper proposes a UAV target positioning algorithm based on particle swarm circle detection algorithm， which preprocesses the image through OpenCV visual module， and obtains parameters such as the core coordinates and radius of the target through particle swarm circle detection algorithm combined with incremental PID （Proportion Integration Differentiation） and filtering.The calibration efficiency and accuracy of UAV are improved by real-time control of motion and the increase of judgment conditions. Experimental results show that the proposed algorithm can control the position adjustment time of the UAV within 4 seconds， and increase the calibration accuracy to 90%， which greatly shortens the UAV calibration time and improves the UAV position calibration accuracy.

Keywords： particle swarm circle detection; incremental PID algorithm; image processing; target positioning; motion control

1? ?引言（Introduction）

隨著人工智能和集成電路等前沿技術的快速發(fā)展，智能系統(tǒng)廣泛應用于各個領域，智能產品也更加受到人們的青睞。四旋翼飛行器作為集傳感器、計算機、自動控制等多學科領域為一體的智能化交叉產品，可適用于各種飛行場景，具有靈活度高、安全性好的特點[1]。

一般來說，當前四旋翼無人機的自主定位方法分為四種：基于慣性傳感器自主定位方法、基于GPS自主定位方法、基于慣性傳感器與GPS組合自主定位方法、基于機器視覺自主定位方法[2]。由于機器視覺具備定位精度高、成本低、邏輯簡單等特點，近年來國內外對無人機在機器視覺自主定位方向的研究也逐漸深入。2014 年，QUINTERO等人利用單目攝像頭結合博弈論、隨機最優(yōu)控制等理論實現(xiàn)了無人機對隨機地面目標的識別跟蹤[3];同年，中國科學技術大學的鄭偉研究了基于特定雙圓目標、單目視覺的飛行器定點懸停，以及基于光束平差法的飛行器導航算法[4]。

本文針對基于機器視覺定位的室內智能無人機，提出了一種新的飛行控制算法。實驗測試結果顯示，該飛行控制方式能使無人機在4 s內完成自主定位，并能夠將定位精度提升到90%左右;使無人機系統(tǒng)可以實現(xiàn)實時高精度的目標識別，為無人機在未來無人配送、目標識別以及工業(yè)機器人等應用方向的研究打下了理論和實踐基礎。

2? ?無人機系統(tǒng)硬件設計（System hardware design of UAV）

2.1? ?系統(tǒng)控制器模塊

本系統(tǒng)采用ARM公司生產的STM32F407VET6為飛行控制器的主控芯片，控制無人機系統(tǒng)四路PWM信號輸出，以維持整個機身飛行的平順性。如圖1所示為本系統(tǒng)采用的飛行控制器設計模塊。采用STM32F407VET6為主控芯片，控制物料投放裝置應用DMA技術獲取二維激光雷達實時返回數(shù)據，并能通過上位機實時監(jiān)測飛行時的加速度、角速度、磁通量等數(shù)據。該型號的芯片成本低，外設資源豐富，功耗低，運算速率快。

2.2? ?系統(tǒng)供電電路設計

本系統(tǒng)采用4S航模鋰電池供電，標稱電壓為14.8 V。鑒于電調工作時電流較大，故而采用并聯(lián)形式進行分流。為了保證樹莓派、飛行控制器等正常工作，另外使用DC/DC降壓模塊完成5 V電壓的輸出。圖2為無人機的電能流向圖。

3? ?飛行控制算法（Flight control algorithm）

無人機在飛行時會受到返回氣流等外界因素的影響，且這些外部影響往往是不可抗拒的[5]，故而需要光流、對地激光等傳感器實時返回數(shù)據至飛行控制器。后者再通過算法解算，向四個電調輸出PWM信號，動態(tài)調整電機轉速，實現(xiàn)無人機定點懸停功能。因此，在OpenCV模塊精準定位圓靶位置后，仍需通過算法調整機身位置，直至穩(wěn)定降落到投放區(qū)域為止。

3.1? ?目標視覺處理

相比于簡單的HSV直方圖、高斯模糊等圖像處理方法，本設計采用一種能夠對圖像進行檢測，同時結合高斯濾波器的導向濾波算法對標靶圖像的噪聲進行濾波[6]。由此算法處理后的圖形如圖3（a）所示，再經過去噪、自適應閾值處理后，可得到二值化處理后如圖3（b）所示的標靶圖像，由圖3可見處理效果明顯。

在得到幀率較高的二值化圖像后，無人機需要在5—6 s實現(xiàn)“穩(wěn)、精、準”地飛行至圓心正上方。因此，快速定位圓心點、實時返回像素坐標值至飛行控制器便顯得尤為重要。由于霍夫圓檢測方法的計算量大、耗時較長，故而使用一種粒子群圓檢測的算法。該算法通過設置最大迭代次數(shù)與閾值等參數(shù)，解決粒子陷入局部最優(yōu)等問題，以此檢測出最佳圓的位置與半徑[7]。通過該算法檢測最小的圓，將其用紅色邊緣線框出，并將圓心用紅點標出，效果如圖4所示。通過多次實驗測試可知，使用該算法能在得到同等結果的條件下耗時更短，進而加快無人機執(zhí)行任務的速度。

3.2? ?相對坐標調整

無人機定位過程中，視覺模組將對拍攝獲得的每一幀圖像進行處理。一般地，以圖像左上頂點作為原點O建立以像素為單位的u-v坐標系，像素點在此坐標系下的u坐標值、v坐標值分別代表像素點所在的行數(shù)與列數(shù)。

同時，要精確標定無人機的目標點，還需要引入以物理單位（例如毫米）表示的圖像坐標系x-y。一般地，以相機光軸與圖像平面的交點（一般位于圖像平面的中心，也稱作圖像主點）作為原點，并且x軸、y軸分別與u軸、v軸平行。設、分別為x軸、y軸的物理尺寸，假設原點在u-v坐標系下的坐標為，則圖像中每個像素點在u-v坐標系與x-y坐標系之間有如下關系：

圖5顯示了在某定點投遞目的的識別中的圖像坐標系，通過公式（1）便可得到目標像素點在x-y坐標系下的坐標值;再通過實際測量中心圓環(huán)的半徑，由小孔成像原理便可獲得無人機定點時x方向、y方向上的可接受誤差距離、;最后通過公式（2），便可獲得u-v坐標系下無人機定點時可接受的誤差像素點數(shù)、。

3.3? ?增量式PID及數(shù)據濾波

在OpenCV將目標坐標點返回至飛行控制器后，后者會根據坐標點進行姿態(tài)調整，進而移動至目標點完成投放。此時要借助增量式PID作為系統(tǒng)自調整方法，完成無人機姿態(tài)的實時變換。增量式PID相較于普通的PID方法，其控制增量的改變量只與三次采樣值相關，通過簡單的加權處理就能夠得到較好的效果，對系統(tǒng)的算力要求更低[8]。此外，增量式PID規(guī)避了系統(tǒng)累計誤差可能造成系統(tǒng)崩潰的問題，更加穩(wěn)定、可靠。

公式（3）中，為第次所得到的誤差，為常系數(shù)，、為周期，為控制增量。

然而，在實際物料運輸中，僅依靠增量式PID算法會導致物料投放位置在一定范圍內出現(xiàn)隨機性，可重復性較差。因此，綜合考慮機身機械結構、環(huán)境風力等因素的影響，本設計提出一種median-average濾波方法。此濾波方法能夠在一定時間內迅速移動至目標點并且減小環(huán)境以及自身難以完全自穩(wěn)的影響——使用“中位值濾波”+“算數(shù)平均濾波”的濾波方法對增量式PID計算得到的姿態(tài)解算速度進行濾波[9]，即對每n 個數(shù)據進行排序后去除其中的最大值與最小值，再對剩下的數(shù)據求取平均值，返回飛行控制器實時的速度值并傳遞給飛控。進而，無人機會以該速度進行位置調整。在下一次讀取數(shù)據進行計算時，所得到的數(shù)據會作為n 個數(shù)據中的最后一個，同時為了保證數(shù)據個數(shù)的一致性，系統(tǒng)會自動將上次所得數(shù)據的第一個去除。

其中，第k 次的n 個數(shù)據為：;、分別代表第k 次得到的n 個數(shù)據中的最大值、最小值。

該種濾波方式可以消除系統(tǒng)存在的偶然誤差及噪聲，對于無人機的運動控制而言是較為合理的。

下面以u-v坐標系中u軸方向，并假設目標u=50、數(shù)據濾波n=5進行Simulink仿真，可得如圖6所示的定位中坐標變化仿真圖。從圖6可以得出：系統(tǒng)在6 s的時間內即可完成動態(tài)調整，保持在目標量附近微小振蕩。

3.4? ?完成判斷條件

在保證精準性的前提下，也應考慮到無人機在執(zhí)行任務中的高效性。因此，需要增加一個判斷條件，即：累計坐標值保持在可接受的誤差范圍內的計數(shù)值，反復更新每次計數(shù)值的增加量，若兩者同時滿足條件則無人機開啟降落模式。

通過公式（2）推導得到無人機在一次實驗條件下u-v坐標系中可接受的誤差像素=5，假設無人機總體計數(shù)值超過140、單次計數(shù)值增長量超過50，則滿足判斷條件直接降落。如圖7所示，設右方參考軸數(shù)值代表計數(shù)值，（i=1，2，3，4）段表示OpenCV獲取的坐標在誤差范圍內，在無人機識別到目標圖像、動態(tài)調整約4 s后，坐標值已經能完全保持在誤差范圍內波動?？紤]到風速、機體自身機械結構等影響，在實際情況下無人機難以如圖7所示將誤差維持在更小的范圍內，因此加入判斷條件不僅能提升無人機的定點效率，也能在一定程度上提高無人機定點的精確性。

3.5? ?降落投放物塊

無人機飛行到指定位置附近并調整位置使其滿足判斷條件后，會觸發(fā)一鍵降落指令。在降落完畢后，以STM32F407VET6為核心的飛行控制器會向主控芯片發(fā)送“OK”指令。主控芯片接收到信號后，會輸出一路PWM波信號，驅動投放裝置中的9g舵機旋轉90°，使物塊平穩(wěn)落在標定點。

4? ?測試結果（Test result）

4.1? ?測試場地圖設計

為測試本文提出的室內無人機運動控制方法的精確性與高效性，特別設置了如圖8所示的測試場地圖。場地中包含邊長為500 mm的正方形起降區(qū)“H”，最大圓環(huán)直徑為500 mm、最小圓環(huán)直徑為100 mm的靶環(huán)區(qū)（共5 環(huán)）。每次測試中，圓靶中心點A將在邊長為600 mm的虛形邊框內隨機取點，無人機將從起降區(qū)“H”處起飛，向前飛行直至識別到紅色的靶環(huán)，然后通過本文提出的運動控制方法標定圓靶中心，最后降落投放測試物塊，記錄物塊落點所在的環(huán)數(shù)為一次有效測試成績（落點在最小圓環(huán)內記5 環(huán)，向外依次遞減）。

4.2? ?測試無人機飛行控制流程圖

圖9為飛行控制流程圖，通過一系列的姿態(tài)解算與位置調整，該無人機能夠實現(xiàn)迅速識別、精確定位，并穩(wěn)定降落至目標點處投放物塊。

4.3? ?測試結果

用于測試的無人機軸距為420 mm，且搭配價格適中的電機、電調進行物塊投放實驗，以驗證本文所提出的飛行控制方法對無人機位置標定的準確率。為避免偶然誤差，以5 次投放的環(huán)數(shù)加和作為一次實驗的考核標準，記錄在表1中。

由表1數(shù)據可見，通過上述的飛行運動控制方法可以使無人機90%以上都能將測試物塊投放在圓環(huán)中心（即視作5 環(huán)），僅有少數(shù)幾次投在了4 環(huán)。從總體標定準確率來看，證明通過本文提出的室內無人機飛行控制算法，可以將定位精度維持在90%左右。

5? ?結論（Conclusion）

本文針對基于機器視覺的室內智能無人機，提出了一種新的飛行控制算法。首先利用導向濾波算法對標靶圖像進行濾波處理，然后利用優(yōu)化的粒子群圓檢測算法獲得標靶中心位置。為了更好地實現(xiàn)無人機的空中自主定位，通過相對坐標調整，獲得無人機可接受的定位誤差像素點，利用增量式PID和數(shù)據濾波算法，實現(xiàn)在短時間內完成動態(tài)調整獲得定位坐標，保證定位坐標的精確性，最終實現(xiàn)無人機的準確定位和物資投遞。實驗測試結果顯示，該飛行控制方式能使得無人機在4 s內完成自主定位，并能夠將定位精度提升到90%左右;使無人機系統(tǒng)可以實現(xiàn)實時高精度的目標識別，為無人機在未來無人配送、目標識別以及工業(yè)機器人等應用方向的研究打下了理論和實踐基礎。

參考文獻（References）

[1] 鞏夢巖，孟青，程思強，等.基于STM32的四旋翼自主投靶飛行器[J].軟件，2018，39（11）：116-119.

[2] 周克旻.基于Kinect室內四旋翼無人機的定位跟蹤與姿態(tài)估計[D].北京：華北電力大學，2019.

[3] QUINTERO S A P， HESPANHA J P. Vision-based target tracking with a small UAV： Optimization-based control strategies[J]. Control Engineering Practice， 2014（32）：28-42.

[4] 鄭偉.基于視覺的微小型四旋翼飛行機器人位姿估計與導航研究[D].合肥：中國科學技術大學，2014.

[5] 吳曉陽.基于OpenCV的運動目標檢測與跟蹤[D].杭州：浙江大學，2008.

[6] 劉志強，孫懷遠，宋曉康.基于機器視覺的藥品包裝濾波算法實現(xiàn)[J].軟件，2018，39（09）：93-96.

[7] 李福慶，蘇湛.粒子群圓檢測算法[J].軟件導刊，2018，17（01）： 60-63.

[8] 王祎晨.增量式PID和位置式PID算法的整定比較與研究[J].工業(yè)控制計算機，2018，31（05）：123-124.

[9] 張龍青，陶娟娟，張羊鴻，等.基于中位值平均濾波算法和OLED屏的PM2.5空氣質量檢測系統(tǒng)設計[J].信息通信，2016（12）：79-80.

作者簡介：

李文強（2000-），男，本科生.研究領域：嵌入式開發(fā).

王嘉棟（2001-），男，本科生.研究領域：嵌入式開發(fā).

隋國榮（1974-），男，博士，副教授.研究領域：光電檢測，機器視覺.本文通訊作者.