基于粒子群圓檢測的無人機(jī)目標(biāo)定位算法

李文強(qiáng)　王嘉棟　隋國榮

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

關(guān)鍵詞：粒子群圓檢測;增量式PID算法;圖像處理;目標(biāo)定位;運(yùn)動控制

中圖分類號：TP272? ? ?文獻(xiàn)標(biāo)識碼：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）

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

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

本文針對基于機(jī)器視覺定位的室內(nèi)智能無人機(jī)，提出了一種新的飛行控制算法。實(shí)驗(yàn)測試結(jié)果顯示，該飛行控制方式能使無人機(jī)在4 s內(nèi)完成自主定位，并能夠?qū)⒍ㄎ痪忍嵘?0%左右;使無人機(jī)系統(tǒng)可以實(shí)現(xiàn)實(shí)時(shí)高精度的目標(biāo)識別，為無人機(jī)在未來無人配送、目標(biāo)識別以及工業(yè)機(jī)器人等應(yīng)用方向的研究打下了理論和實(shí)踐基礎(chǔ)。

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

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

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

2.2? ?系統(tǒng)供電電路設(shè)計(jì)

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

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

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

3.1? ?目標(biāo)視覺處理

相比于簡單的HSV直方圖、高斯模糊等圖像處理方法，本設(shè)計(jì)采用一種能夠?qū)D像進(jìn)行檢測，同時(shí)結(jié)合高斯濾波器的導(dǎo)向?yàn)V波算法對標(biāo)靶圖像的噪聲進(jìn)行濾波[6]。由此算法處理后的圖形如圖3（a）所示，再經(jīng)過去噪、自適應(yīng)閾值處理后，可得到二值化處理后如圖3（b）所示的標(biāo)靶圖像，由圖3可見處理效果明顯。

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

3.2? ?相對坐標(biāo)調(diào)整

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

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

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

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

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

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

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

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

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

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

3.4? ?完成判斷條件

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

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

3.5? ?降落投放物塊

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

4? ?測試結(jié)果（Test result）

4.1? ?測試場地圖設(shè)計(jì)

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

4.2? ?測試無人機(jī)飛行控制流程圖

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

4.3? ?測試結(jié)果

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

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

5? ?結(jié)論（Conclusion）

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

參考文獻(xiàn)（References）

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

[2] 周克旻.基于Kinect室內(nèi)四旋翼無人機(jī)的定位跟蹤與姿態(tài)估計(jì)[D].北京：華北電力大學(xué)，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] 鄭偉.基于視覺的微小型四旋翼飛行機(jī)器人位姿估計(jì)與導(dǎo)航研究[D].合肥：中國科學(xué)技術(shù)大學(xué)，2014.

[5] 吳曉陽.基于OpenCV的運(yùn)動目標(biāo)檢測與跟蹤[D].杭州：浙江大學(xué)，2008.

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

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

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

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

作者簡介：

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

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

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