基于機(jī)器視覺的四軸飛行器自主巡航算法改進(jìn)

胡雨豪，劉 波,2

（1. 湖南農(nóng)業(yè)大學(xué)信息科學(xué)技術(shù)學(xué)院，湖南 長沙 410128；2. 湖南省農(nóng)村農(nóng)業(yè)信息化工程技術(shù)研究中心，湖南 長沙 410128）

0 引言

隨著國家持續(xù)對“三農(nóng)問題”的關(guān)注，實(shí)施鄉(xiāng)村振興戰(zhàn)略的重要性更加突出，農(nóng)業(yè)信息化建設(shè)需求也在不斷提升[1]，在農(nóng)業(yè)信息化建設(shè)過程中，病蟲害是影響農(nóng)作物產(chǎn)量和質(zhì)量的關(guān)鍵因素之一[2]，直接影響國家糧食安全與民生工程[3]。作為農(nóng)業(yè)大國，農(nóng)業(yè)病蟲害的發(fā)生對我國每年農(nóng)作物收成造成了巨大的損失[4]，而且當(dāng)前農(nóng)田病蟲害的防治大部分依靠人力進(jìn)行，勞動任務(wù)繁重，對作業(yè)實(shí)施者傷害性也不小。要提高災(zāi)害防治效率，其中的一個重要環(huán)節(jié)是實(shí)時獲取農(nóng)業(yè)區(qū)域的數(shù)據(jù)[5]，基于農(nóng)作物災(zāi)害防治自身的一些條件限制，一些大型機(jī)械難以發(fā)揮其主要功能，而四軸飛行器的應(yīng)用可以克服這些缺點(diǎn)，本項目主要研究STM32單片機(jī)作為主控的智能農(nóng)用飛行器，集計算機(jī)、機(jī)械、軟件仿真、算法設(shè)計等多種技術(shù)于一體，利用搭載在四軸飛行器上的5.8 G遠(yuǎn)程圖像傳輸模塊[6]，通過顯示屏觀察回傳過來的圖像，替代人工巡邏等情況，相比于其他機(jī)器，具有監(jiān)測數(shù)據(jù)精確度高、時效性好的優(yōu)點(diǎn)[7]，極大的提高了災(zāi)害防治效率。

1 研究背景

隨著科技的發(fā)展，互聯(lián)網(wǎng)的漸漸壯大，信息技術(shù)在發(fā)生著巨大的變化[8]，機(jī)器視覺和圖像處理技術(shù)己經(jīng)在農(nóng)業(yè)信息化建設(shè)方面得到了廣泛應(yīng)用。在現(xiàn)代農(nóng)業(yè)的發(fā)展過程中，農(nóng)產(chǎn)品的生產(chǎn)過程對于其質(zhì)量而言，有著關(guān)鍵性的作用[9]。如何實(shí)時、高效率地獲取相關(guān)信息是新時代農(nóng)業(yè)生產(chǎn)、管理的一個重要問題，但通過衛(wèi)星監(jiān)測地面樣方存在效率不高、樣方面積較小的問題[10]，單純依靠人工采樣和有線測量的方式獲取數(shù)據(jù)在實(shí)時性、精準(zhǔn)性和實(shí)用性等方面也無法滿足現(xiàn)代農(nóng)業(yè)信息化建設(shè)的要求[11]。將機(jī)器視覺和圖像處理技術(shù)應(yīng)用于農(nóng)業(yè)信息化建設(shè)，對實(shí)時監(jiān)測農(nóng)作物的長勢、災(zāi)害的防治以及果實(shí)檢測分級中有著至關(guān)重要的作用。

農(nóng)業(yè)領(lǐng)域中進(jìn)行大面積、高效率的作業(yè)往往需要采用專業(yè)的無人駕駛飛機(jī)對農(nóng)作物的生長以及災(zāi)害情況進(jìn)行實(shí)時、動態(tài)的監(jiān)測，這就決定了機(jī)器視覺和動態(tài)圖像處理技術(shù)在今后的大面積農(nóng)業(yè)作業(yè)中更為高效實(shí)用，因此本文分析了機(jī)器視覺和圖像處理技術(shù)以及四軸飛行器在大面積農(nóng)業(yè)作業(yè)上的優(yōu)勢，采用了市面上最新出現(xiàn)的機(jī)器視覺模塊openmv，并基于四軸飛行器進(jìn)行了室內(nèi)自主巡航系統(tǒng)的開發(fā)和研究，很好的模擬了飛行器在巡航過程中對地面環(huán)境的檢測，為無人機(jī)作業(yè)過程在線檢測、害蟲監(jiān)控、農(nóng)產(chǎn)品分級等諸多方面的發(fā)展提供借鑒。

2 PID控制算法

PID，為比例、積分、微分的對應(yīng)英文首字母的簡寫，是現(xiàn)在應(yīng)用的最為廣泛的工業(yè)控制器，四旋翼飛行器常用的飛行姿態(tài)控制算法是PID算法[12]，PID算法作為最經(jīng)典的控制算法，它的公式如下：

公式中：Kp乘以誤差e(t)，用來消除當(dāng)前誤差；積分項系數(shù)Ki乘以誤差e(t)的積分，用來消除累積的誤差，可以達(dá)到無差調(diào)節(jié)；微分項系數(shù) Kd乘以誤差 e(t)的微分，用來消除誤差的變化，也就是確保誤差恒定不變。

P的作用是加快反饋調(diào)節(jié)的響應(yīng)速度，增強(qiáng)整體的準(zhǔn)確性，盡可能消除誤差[13]。參數(shù)kp的大小與響應(yīng)速度成反比。在調(diào)試過程中，應(yīng)從0開始增加kp值，直到出現(xiàn)系統(tǒng)震蕩，再逐漸減小 kp至震蕩消失，記錄下kp的值。

單單只依靠p來調(diào)節(jié)，雖然能夠快速消除誤差，有較高的相應(yīng)速度，然而這樣使得系統(tǒng)存在著穩(wěn)定誤差。I引入的作用便是為了消除穩(wěn)態(tài)溫差。I的參數(shù)ki的調(diào)節(jié)過程與kp類似；I的引入，雖然消除了穩(wěn)態(tài)溫差，但也影響了系統(tǒng)的穩(wěn)定性，并且PI控制都是在誤差產(chǎn)生后才進(jìn)行回調(diào)的，是事后調(diào)節(jié)；引入微分D的作用則是事前預(yù)防控制，對誤差的大小進(jìn)行一個提前預(yù)測，并產(chǎn)生一個抑制行為；kd的值一般設(shè)置為0即可。圖1為PID控制器的基本結(jié)構(gòu)。

飛行器的姿態(tài)控制是四軸飛行器核心技術(shù)所在，而姿態(tài)的控制在于控制算法的優(yōu)良，目前有智能控制算法和PID控制算法，然而智能控制算法會受到微處理器計算能力限制而不能達(dá)到實(shí)時控制的良好效果，PID則不同，PID控制算法相對簡單，且技術(shù)成熟，是目前控制算法的首選[14]。PID算法即是PID控制器的靈魂，其對多旋翼飛行器的重要性在此可見一斑。

3 算法優(yōu)化

3.1 串級PID

串級PID由兩個主、副兩個PID控制器組成[15]，將它們串聯(lián)起來使用。基于四旋翼的串級PID分為內(nèi)環(huán)和外環(huán)，外環(huán)對位置進(jìn)行控制，內(nèi)環(huán)則對角度進(jìn)行控制，并將外環(huán)的控制結(jié)果傳給內(nèi)環(huán)作為內(nèi)環(huán)的輸入給定值，并與實(shí)際輸出值作比較，進(jìn)行控制調(diào)節(jié)。這個控制器的好處就在于內(nèi)環(huán)對系統(tǒng)的響應(yīng)速度快，控制干擾能力強(qiáng)。采用串級PID方法可以更好的控制系統(tǒng)穩(wěn)定。

圖1 PID控制原理圖Fig.1 Schematic diagram of PID control

PID一般分為位置式和增量式兩種，由于計算機(jī)輸出的u(k)直接去控制電機(jī)的油門，u(k)的值與電機(jī)的油門一一對應(yīng)，所以這里采用位置式 PID。如下給出位置式PID的公式。

PID算法的離散形式為：

其中：T——采樣周期，K——采樣序號，e(k)——第 k次采樣時刻輸入的偏差值，

基本流程是遙控器的控制量經(jīng)過換算得到歐拉角的期望值，飛行器經(jīng)過陀螺儀測量機(jī)體實(shí)際角度經(jīng)過而誤差就是期望角度與實(shí)際角度的差值，將得到的數(shù)值經(jīng)過PID控制器的運(yùn)算輸出給副控制器，與陀螺儀的采樣角速度做差，結(jié)果作為副控制器的輸入?yún)?shù)，經(jīng)過PID運(yùn)算后輸出結(jié)果限幅后作為控制量送給油門調(diào)節(jié)通道。

3.2 算法優(yōu)化與應(yīng)用

3.2.1 串級PID算法應(yīng)用

本系統(tǒng)采用了串級PID控制算法對飛行器姿態(tài)進(jìn)行自動化控制，外環(huán)控制角度使用PID控制器，提高控制精度，內(nèi)環(huán)控制角速度使用 PD控制器產(chǎn)生阻尼效果，防止調(diào)節(jié)過快導(dǎo)致震蕩，優(yōu)化飛行效果。兩級PID均采用位置式PID，高度控制采用和角度控制一樣的雙環(huán)PID，定點(diǎn)PID控制采用單級PID中的P。串級PID控制算法具有抗干擾能力強(qiáng)，變化靈活等特點(diǎn)，因為有兩個控制器控制飛行器，相比于一個控制器能控制更多的變量，這使得飛行器的適應(yīng)能力變得更強(qiáng)。基本控制流程如圖2所示。

圖2 PID控制流程圖Fig.2 Flow chart of PID control

3.2.2 定點(diǎn)、自主巡航算法優(yōu)化

系統(tǒng)用于飛行控制數(shù)據(jù)處理的上位機(jī)為匿名地面站，定高采用超聲波和氣壓計融合定高，定點(diǎn)和自主巡航采用 openmv模塊，通過對比回傳回來的圖像得到飛行器在一段時間內(nèi)的運(yùn)動軌跡，從而推算出飛行的距離，速度以及方向。

定點(diǎn)的實(shí)現(xiàn)與優(yōu)化主要采用 openmv庫中的find_blobs函數(shù)尋找特定的色塊并識別。通過在openmv的 IDE軟件中設(shè)置顏色閾值來尋找感興趣的色塊，算法默認(rèn)找出圖像中最大面積的色塊，并利用 openmv庫中的 draw_rectangle函數(shù)和 draw_cross函數(shù)畫十字和矩形來標(biāo)記出色塊的位置，最后以20 ms/次的發(fā)送速率將位置信息發(fā)送給飛控，飛控對其進(jìn)行數(shù)據(jù)處理后進(jìn)行位置PID控制，使目標(biāo)色塊始終保持在攝像頭的圖像中心位置，從而實(shí)現(xiàn)定點(diǎn)。控制流程如圖3所示。

圖3 定點(diǎn)控制流程圖Fig.3 Flow chart of fixed-point control

自主巡航的優(yōu)化通過設(shè)置 5個取樣區(qū)域 ROI(x,y,w,h,weight)，代表圖像中左上頂點(diǎn)(x,y)寬，高分別為w和h的矩形，weight為當(dāng)前矩形的權(quán)值。矩形的閾值根據(jù)實(shí)際情況調(diào)整，離攝像頭視野最近的矩形權(quán)值最大，其主要優(yōu)化過程如下：

1. 路線識別

利用顏色識別尋找目標(biāo)區(qū)域內(nèi)的線段，由于目標(biāo)區(qū)域內(nèi)的顏色塊（線段塊）不止一個，算法默認(rèn)找到面積最大的一個作為本區(qū)域內(nèi)的目標(biāo)路線，并將此區(qū)域像素數(shù)最大的顏色塊用矩形和十字標(biāo)記出來。

2. 坐標(biāo)轉(zhuǎn)換

首先利用中間公式計算目標(biāo)路線的中心坐標(biāo)，公式如下：

center_pos = (centroid_sum / weight_sum)

centroid_sum--每個區(qū)域的最大顏色塊的中心點(diǎn)的x坐標(biāo)值乘本區(qū)域的權(quán)值。

weight_sum—矩形的權(quán)值，再通過轉(zhuǎn)換公式得到偏轉(zhuǎn)值，由于這里得到的偏轉(zhuǎn)值為弧度值，所以利用openmv庫中的math函數(shù)將其轉(zhuǎn)換為飛行器應(yīng)偏轉(zhuǎn)的角度并發(fā)送給飛控。

3. 巡航控制

當(dāng)攝像頭圖像中全部為目標(biāo)色塊時（如圖4），可將圖像劃分為五個區(qū)域，其中左邊和右邊兩個矩形決定飛行器y軸的中心，上邊和下邊兩個矩形決定飛行器x軸的中心，當(dāng)四個矩形同時出現(xiàn)表示攝像頭圖像中四個區(qū)域的色塊都可以找到并識別，此時確定攝像頭圖像的中心，用圓形標(biāo)記出來。

圖4 算法原理模型Fig.4 Model of algorithm principle

當(dāng)飛行器直線巡航時，只需要左右兩個矩形對y軸方向進(jìn)行糾正即可；當(dāng)飛行器需要進(jìn)行轉(zhuǎn)彎判斷時，即上邊的矩形消失，若左邊的矩形同時消失，飛行器右轉(zhuǎn)，反之左轉(zhuǎn)。流程圖5所示。

4 系統(tǒng)測試

4.1 定點(diǎn)測試

黑膠布在長寬為 100*120 cm的白色背景圖紙上面貼出直徑為10 cm的黑色實(shí)心圓作為定點(diǎn)測試的目標(biāo)點(diǎn)（因為這樣無論從哪個角度起飛都是對稱的）。在測試之前，在飛行器下面偏前面安裝一個紅外發(fā)射頭，以便更好的觀察定點(diǎn)及巡航效果。將飛行器放在黑色實(shí)心圓的中心，上電之后 openmv開始工作，openmv不斷拍攝圖像并通過串口回傳數(shù)據(jù)給飛控，然后解鎖將油門緩緩抬高，使飛行器平穩(wěn)離地50厘米并進(jìn)行定高，當(dāng)飛行器穩(wěn)定后，開啟定點(diǎn)功能，此時飛行器穩(wěn)定在黑色實(shí)心圓上方，無飄走現(xiàn)象；使用遙控器施加一定程度干擾，飛機(jī) 1~2秒內(nèi)能自行飄回實(shí)心圓上方，飛行器有一定抗干擾能力，定點(diǎn)效果良好。

圖5 巡航控制流程圖Fig.5 Flow chart of cruise control

4.2 自主巡航測試

用黑膠布在長寬為 100*120 cm的白色背景圖紙上貼出線寬為5 cm，長寬為80*100 cm的矩形路線作為巡線導(dǎo)航的飛行路線。和定點(diǎn)時的步驟一樣，將飛行器放在起飛點(diǎn)中心，上電解鎖以后，使飛行器平穩(wěn)起飛至50 cm高定高定點(diǎn)，然后開啟自主導(dǎo)航功能，飛行器穩(wěn)定5秒鐘后開始巡線飛行，按矩形路線飛行一圈后在起飛點(diǎn)穩(wěn)定5秒鐘后降落。巡線過程中可以看到，飛行器直線飛行時始終保持在黑線上方，無明顯飄走現(xiàn)象，轉(zhuǎn)彎偶爾有飄走現(xiàn)象，但很快就能回到路線上面，有很好的抗干擾能力。

4.3 自動記錄飛行軌跡

將飛行器放在起飛點(diǎn)，上電起飛后，PC接收飛行器上圖傳回傳過來的數(shù)據(jù)，通過分析一系列圖像序列，來確定飛行器的朝向和位置，通過 FAST特征檢測以及KLT跟蹤算法提取每一幀圖像的特征點(diǎn)并進(jìn)行特征匹配，最終確定飛行器軌跡并在 PC端顯示。部分軌跡圖6所示。

5 結(jié)論

本文說明了四軸飛行器自主巡航系統(tǒng)的軟硬件實(shí)現(xiàn)方案，openmv脫機(jī)運(yùn)行的特性大大降低了基于嵌入式設(shè)備的室內(nèi)定定點(diǎn)及自主巡航的技術(shù)難度，同時飛行器在定點(diǎn)和巡航中偏移目標(biāo)區(qū)域的概率極小，具有良好的精度和穩(wěn)定性，其良好的抗干擾能力給自主監(jiān)測農(nóng)業(yè)作物災(zāi)害研究提供了可靠的依據(jù)。

[1] 劉波, 沈岳, 郭平, 林芳. 數(shù)字湖南農(nóng)業(yè)信息化建設(shè)關(guān)鍵策略探索[J]. 軟件, 2013, 34(10): 1-6.

[2] 趙希萌. 無人機(jī)在現(xiàn)代農(nóng)業(yè)工程中的應(yīng)用[J]. 科技創(chuàng)新與應(yīng)用, 2016(36): 291-291.

[3] 黃天天, 劉波, 沈岳. 農(nóng)業(yè)物聯(lián)網(wǎng)雙光譜熱成像圖像融合模型設(shè)計[J]. 物聯(lián)網(wǎng)技術(shù), 2015, 5(07): 26-28.

[4] 蔣暢, 劉凡齊. 四軸飛行器在農(nóng)業(yè)上應(yīng)用的研究[J]. 中國新通信, 2017, 19(7): 95-95.

[5] 陳曉燕, 姚高偉, 張鯤, 等. 基于遺傳算法的無線傳感器節(jié)點(diǎn)定位在農(nóng)業(yè)的應(yīng)用[J]. 軟件, 2015, 36(4): 1-5.

[6] Nyjot, Tshikala, 黃李炳, 等. 基于GPS/INS導(dǎo)航的農(nóng)用無人機(jī)航點(diǎn)規(guī)劃算法研究及應(yīng)用[J]. 成組技術(shù)與生產(chǎn)現(xiàn)代化,2017, 34(2): 9-11.

[7] 張志云, 李長賀. 無人機(jī)技術(shù)在現(xiàn)代農(nóng)業(yè)中的應(yīng)用[J]. 農(nóng)業(yè)工程, 2016, 6(4): 23-25.

[8] 胡毅, 劉波. 基于農(nóng)業(yè)信息化知識檢索系統(tǒng)關(guān)鍵技術(shù)分析與設(shè)計[J]. 軟件, 2011, 32(8): 9-12.

[9] 傅俊, 曹春益. 基于物聯(lián)網(wǎng)的農(nóng)產(chǎn)品質(zhì)量溯源系統(tǒng)設(shè)計[J].軟件, 2014(3): 9-10.

[10] 李琳一, 袁濤, 陳旭. 農(nóng)業(yè)抽樣調(diào)查中統(tǒng)計理論和抽樣方法選取研究[J]. 上海農(nóng)業(yè)學(xué)報, 2011, 27(4): 5-8.

[11] 李震, 洪添勝. 無線傳感器網(wǎng)絡(luò)技術(shù)在精細(xì)農(nóng)業(yè)中的應(yīng)用進(jìn)展[J]. 湖南農(nóng)業(yè)人學(xué)學(xué)報: 自然科學(xué)版. 2011. 37.

[12] 王欣. 基于四旋翼飛行器的PID算法改進(jìn)[J]. 電子技術(shù)與軟件工程, 2016(4): 161-161.

[13] 林明寶, 余春艷. 自主巡航的四軸飛行器控制系統(tǒng)的設(shè)計與實(shí)現(xiàn)[J]. 電腦知識與技術(shù), 2016, 12(16): 227-230.

[14] 鄒桐. 淺談PID控制算法對四軸飛行器的重要性[J]. 電腦知識與技術(shù): 學(xué)術(shù)交流, 2015, 11(8X): 177-178.

[15] 畢嘉誠, 陳維民, 于曉磊, 等. 基于四軸飛行器的串級PID控制算法與建模[J]. 數(shù)字技術(shù)與應(yīng)用, 2016(11):121-122.