一種基于OpenMV的飛行器激光引導(dǎo)算法

黃迅 崔敏 黃隆群 邱敏卿

摘? 要：一套能夠在短距離內(nèi)獲得飛行器相對于目標(biāo)物的位置，并能引導(dǎo)飛行器飛向目標(biāo)的引導(dǎo)系統(tǒng)，對于當(dāng)下的無人機定位應(yīng)用開發(fā)來說是十分必要的。大多數(shù)固定翼飛行器目前都采用GPS的方式進行引導(dǎo)，但是在實際應(yīng)用中，通常GPS精度只能夠達到數(shù)米級，而更加精確的定位，就必須依靠諸如激光等主動導(dǎo)引方案。文章提出了一種基于OpenMV的固定翼飛行器近距離激光引導(dǎo)算法，在朝目標(biāo)物照射激光后，固定翼飛行器能根據(jù)OpenMV求出的位置偏差量，結(jié)合PID自動調(diào)整自身姿態(tài)，穩(wěn)定地朝激光方向飛行，為飛行器主動引導(dǎo)導(dǎo)航提供了一種可實現(xiàn)的廉價思路。

關(guān)鍵詞：固定翼;定位算法;激光引導(dǎo);OpenMV;PID

中圖分類號：TP242? ? ? ? ?文獻標(biāo)志碼：A? ? ? ? ?文章編號：2095-2945（2019）11-0019-04

Abstract： A guiding system that can obtain the position of the aircraft relative to the target in a short distance and can guide the aircraft to the target is very necessary for the development of the current UAV positioning application. At present， most fixed-wings are currently guided by GPS， but generally， GPS accuracy can only reach several meters. In order to locate more accurately， active guidance schemes such as laser must be relied on. In this paper， a OpenMV-based near-range laser guidance algorithm for fixed-wing aircraft is presented. After the laser is irradiated toward the target， according to this scheme， the fixed-wing aircraft will automatically adjust its posture according to the obtained deviation amount combined with the PID algorithm， and then stabilize the flight towards the target. It proposes an achievable cheap solution for fixed-wing active navigation.

Keywords： fixed wing; positioning algorithm; laser guidance; OpenMV; PID

1 概述

隨著無人機技術(shù)的逐步發(fā)展以及無人機市場的逐步擴大，無人機定位技術(shù)的應(yīng)用場景也越來越多。以常見的四旋翼無人機為例，現(xiàn)在實用化的四旋翼無人機一般都要有光流定點作為室內(nèi)定位，GPS與光流輔助作為室外定位[1]。目前無人機行業(yè)的領(lǐng)軍者大疆甚至開發(fā)出了視覺里程計技術(shù)，實現(xiàn)了極為精確的室內(nèi)/外定位技術(shù)?？梢哉f，不能進行定點定位的無人機是殘缺的，不會被市場所接納。

而固定翼無人機的導(dǎo)航以及定位技術(shù)，目前卻仍然比較單一，大部分還是以GPS導(dǎo)航定位為主，少部分會結(jié)合a的導(dǎo)航方式都屬于衛(wèi)星導(dǎo)航，衛(wèi)星導(dǎo)航受環(huán)境的影響非常大，室內(nèi)或者室外電磁干擾大的時候，衛(wèi)星導(dǎo)航就沒用了，局限性大。因此也需要一種能夠脫離對衛(wèi)星的依賴，但是又能實現(xiàn)導(dǎo)航的引導(dǎo)系統(tǒng)出現(xiàn)。

針對以上問題，筆者提出了一種可以在短距離生效的激光引導(dǎo)方案，該方案基于現(xiàn)在流行的OpenMV，在飛行器飛行過程中，只需給定一束特定的激光束照向目標(biāo)，OpenMV就能發(fā)現(xiàn)激光束的位置并生成對應(yīng)的控制信號引導(dǎo)飛行器朝目標(biāo)航向飛行。這種方案能夠擺脫固定翼飛機導(dǎo)航對衛(wèi)星的依賴，在短距離導(dǎo)航場合有一定的應(yīng)用價值。

2 激光引導(dǎo)系統(tǒng)的總體架構(gòu)

在本例中，激光引導(dǎo)系統(tǒng)主要由紅色激光照射器、固定翼飛行器、APM開源飛控、姿態(tài)調(diào)整執(zhí)行器和OpenMV開源圖像識別模塊等五部分組成。

2.1 紅色激光照射器

激光照射器采用市場上常見能買到的530nm、100mw 的紅色激光手電，這種激光手電可以照射出穩(wěn)定的激光并且焦點可調(diào)。

2.2 固定翼飛行器

筆者采用了飛翼構(gòu)型的固定翼飛行器來作為這套制導(dǎo)系統(tǒng)的搭載平臺。如圖1所示。飛翼布局的飛行器相對于正常式布局的飛行器來說具有諸多優(yōu)點。飛翼式的飛機沒有機身，只有一張巨大的機翼，只需要左、右兩片副翼就可以完成包括俯仰、滾轉(zhuǎn)、偏航三個自由度的控制，雖然其本身相對于正常式布局的飛行器來說并不是一種特別穩(wěn)定的結(jié)構(gòu)，但是配合飛控之后其穩(wěn)定性能和正常式布局的固定翼相差無幾。而其簡單的機械結(jié)構(gòu)，方便快速維修的特性，非常適合用于進行可能會造成損毀的固定翼導(dǎo)航飛行試驗。

2.3 APM開源飛控

APM開源飛控來源于著名無人機技術(shù)論壇DIY Drones，是一個歷史悠久，非常成熟的飛控，而其經(jīng)過多年的發(fā)展，目前也發(fā)展出了多種飛控形式。其中，最小最輕，最適合于狹窄空間應(yīng)用的當(dāng)屬Mini APM[2]。

APM飛控“開源”意味著其軟件和硬件都是開源的，而其軟件已經(jīng)有成熟的ArduPilot作為基礎(chǔ)，可供二次開發(fā)，擴展性強。更重要的是，APM原生支持飛翼構(gòu)型的飛行器，為開發(fā)省下了很多功夫。

2.4 姿態(tài)調(diào)整執(zhí)行器

姿態(tài)調(diào)整執(zhí)行器本質(zhì)上即為一個擺動矢量發(fā)動機。它能夠為飛行器的姿態(tài)調(diào)整提供一個瞬態(tài)、衡定和線性的力。而傳統(tǒng)飛翼飛行器調(diào)整姿態(tài)用的副翼在調(diào)整姿態(tài)時容易受當(dāng)前空速影響，不同空速下產(chǎn)生的力也不盡相同，而副翼在偏轉(zhuǎn)時又會反過來影響當(dāng)前飛行器的空速。因此，它對于飛行器位置的影響是一個非線性耦合的問題，不利于用在要求瞬態(tài)反應(yīng)很快的飛行器激光導(dǎo)引上面。

開發(fā)這套方案所采用的擺動矢量發(fā)動機是使用兩個球副連桿和兩組舵機分別控制擺盤的兩個自由度，而擺盤上固定著一個發(fā)動機，它為整個系統(tǒng)提供動力，如圖2所示。

2.5 OpenMV開源圖像識別模塊

OpenMV是一個以STM32F427芯片為核心的開源低成本機器視覺模塊。其利用Python作為編程語言進行編程。而在其內(nèi)部已經(jīng)集成了很多常見的圖像識別算法，比如顏色識別、核相關(guān)濾波、特征檢測等，還支持在增加外置驅(qū)動板之后用于控制舵機。雖然其攝像頭像素比較低，只有30W像素左右，但是對于識別任務(wù)來說已經(jīng)足夠，太多的像素點反而不利于移動芯片進行處理，甚至還要因此做下采樣，使其分辨率降低，增加圖像處理的計算速度。

3 設(shè)計與實驗

3.1 激光信號識別

3.1.1 OpenMV程序的基本結(jié)構(gòu)形式

在本應(yīng)用中，OpenMV的作用就是要識別達到目標(biāo)物上的激光。由于筆者使用的激光是一種紅色的可見激光，因此可以通過OpenMV中自帶的查找色塊函數(shù)直接找出環(huán)境中的激光。

在OpenMV中，程序的執(zhí)行方式是以每幀執(zhí)行一次的方式實現(xiàn)的。換而言之，OpenMV處理視頻流的方式，就是不斷的拍照，然后對每一張拍到的照片進行圖像識別處理，因此，OpenMV中的程序都遵循以下的基本結(jié)構(gòu)形式。

即所有的程序都放在不斷的循環(huán)之中，以循環(huán)的方式進行相應(yīng)的處理。

3.1.2 激光點識別算法

要使飛行器能夠朝激光光點所在的位置正確的飛行，首先就得讓OpenMV能夠識別出激光。在本應(yīng)用中，OpenMV的激光光點識別主要采用顏色識別的方式進行相應(yīng)的識別。

顏色識別是數(shù)字圖像處理領(lǐng)域中最常用的應(yīng)用之一，通過顏色色域的過濾，能夠把環(huán)境中特定顏色的物品分離出來[3]。OpenMV識別到激光光點時其內(nèi)部機器視覺所能看到的效果如圖3所示，左圖為測試環(huán)境，其墻體立柱上的光點即為激光光斑。右圖為OpenMV視野，可以看到激光光斑已經(jīng)被完全分離出來。

在程序?qū)崿F(xiàn)上，筆者采用了OpenMV中自帶的查找視野中對應(yīng)LAB色域色塊的函數(shù)：

其中，red_threshold中存放的是所要查找色塊的色域值，在本應(yīng)用中red_threshold存放的即為激光光斑的LAB色域值。

而采用OpenMV作顏色識別應(yīng)用，還需考慮的一個影響識別率的重要問題就是環(huán)境的光照問題。實驗表明，對相同的物品，在不同的光照環(huán)境下，攝像頭視野中所顯示的物品顏色完全不同，而其機器視覺中所識別到的LAB直方圖成分也有很大差異。

然而，采用激光光斑則不需要關(guān)心這個在大多數(shù)OpenMV顏色識別應(yīng)用中會出現(xiàn)的問題，因為激光本身具有色域固定的性質(zhì)，而且激光光斑通常亮度會比環(huán)境光亮好幾倍[4]，因此也更加容易被攝像頭識別并分離出來。

但是，在實際應(yīng)用中，OpenMV的find_blobs函數(shù)通常機制是會返回環(huán)境中所有包含這種顏色的點的位置以及大小。因此其函數(shù)輸出量中通常會有很多用不到的色塊，實際結(jié)果就會給后續(xù)的處理程序引入了很多“雜波”，圖4（a）所示是find_blobs未經(jīng)濾波時，輸出的激光光點所在位置。

為了去除環(huán)境中的這些雜波，必須引入某種形式上的“濾波器”，去將這些雜波濾除。根據(jù)筆者的實際測試后發(fā)現(xiàn)，可以用查找環(huán)境中最大色塊的辦法來濾去大部分的雜波。因為很多雜波相對于激光的光斑來說其所占的像素值都非常的小，只有1～2個像素，與真正的光斑特征在大小上的相差最為明顯。查找雜波的代碼如下所示：

其中，blobs為find_blobs函數(shù)的輸出量。在這里它作為輸入量輸入到函數(shù)中去，而return max_blob則輸出經(jīng)過查找之后最大色塊所在的位置和大小。由此實現(xiàn)了基礎(chǔ)濾波，濾波效果如圖4（b）所示。

然而在實際試驗時，發(fā)現(xiàn)這種濾波的方法并不足夠好。因為有很多時候顏色的干擾是隨機猝發(fā)的，甚至本來激光光斑被識別在了視野中央，而由于飛行器的突然抖動，光斑會閃到屏幕左上角或者其他位置。而也正是這些猝發(fā)性的干擾有時會對飛行器的導(dǎo)航帶來災(zāi)難性的后果。

為了濾除這些干擾，筆者采取了“幀拋棄”的辦法。即每幀都進行一次本幀光斑所識別到的位置以上一幀的位置進行比對，如果位置相差太大，超過所設(shè)定的閾值，就拋棄掉這一幀，直接識別下一幀，周而復(fù)始，不斷循環(huán)。

根據(jù)實際測試，采用以上兩種濾波辦法，就已經(jīng)能夠濾除大部分的干擾。并且保證飛行器在進行激光識別和導(dǎo)航時不出差錯。

3.2 飛行器導(dǎo)航算法

3.2.1 激光制導(dǎo)思路

在獲得了激光光點的位置后，接下來要做的就是要引導(dǎo)飛行器飛向激光光點所在的位置。由于采用的是視覺導(dǎo)航方式，即此時飛行器上裝載的OpenMV應(yīng)該已經(jīng)能夠“看到”光斑的所在。在這種情況下，筆者認為飛行器此時已經(jīng)是朝向激光所在的位置飛去的，只是需要在擊中之前做一些微調(diào)。

為了能夠精確的擊中激光光斑所在的位置，就必須采取某種調(diào)整控制方法，參考文獻[5]，筆者設(shè)定飛行器的調(diào)節(jié)目標(biāo)，只要把激光光斑的位置調(diào)節(jié)到視界的中心之上，即與飛行器的中軸線重合，這樣就能夠保證飛行器始終朝激光所在位置飛行，并最終擊中激光所在位置。

因此，要調(diào)整飛行器的飛行軌跡，首先要做的就是要計算出激光光斑與飛行器中軸線之間的差值。根據(jù)下面的代碼可以求出這個差值。

其中max_blob.cx（）、max_blob.cy（）為識別出的激光色塊的中心點坐標(biāo)，img.height（）和img.width（）為攝像頭視野的高度和寬度。在求出了俯仰（Pitch）、滾轉(zhuǎn)（Roll）方向的差值之后，需要把這個差值轉(zhuǎn)化為相應(yīng)的控制信號傳遞給控制飛行器矢量電機的兩個舵機，以促使飛行器調(diào)整自身的姿態(tài)。意即需要將位置的偏差量轉(zhuǎn)化為舵機的偏轉(zhuǎn)角度數(shù)值。

而為了更好的控制飛行器姿態(tài)調(diào)整的過程，防止過調(diào)，就必須要考慮偏差的變化速度以及系統(tǒng)靜態(tài)誤差的積累。為了做到這一點，筆者引入了PID調(diào)節(jié)算法用于控制飛行器的位置偏差量。在本應(yīng)用中，筆者采用增量式的PID控制方法，整個系統(tǒng)為一個單級PID，其控制回路框圖如圖5所示。

其中，r（t）為偏差量，e（t）為反饋值c（t）與控制信號加和后的量，u（t）為輸出給飛行器矢量電機的動作信號。

在最初的偏差量進入了PID系統(tǒng)，并得到最終的調(diào)定輸出量之后，其輸出量還需要轉(zhuǎn)化為OpenMV控制舵機所能接受的角度數(shù)值，如下面的代碼所示：

其中，Pitch_servo.angle（）和Roll_servo.angle（）分別設(shè)定矢量電機控制俯仰和滾轉(zhuǎn)舵機的角度，Pitch _output和Roll _output分別為單級PID計算出的俯仰和滾轉(zhuǎn)動作信號。

3.2.2 滾轉(zhuǎn)方向的靈敏度

在實際開發(fā)過程中，還發(fā)現(xiàn)滾轉(zhuǎn)方向姿態(tài)控制具有左滾轉(zhuǎn)和右滾轉(zhuǎn)靈敏度不同的問題，通常，飛行器左滾的趨勢會比右滾更加明顯。經(jīng)過仔細分析之后發(fā)現(xiàn)，之所以靈敏度會不同，是因為驅(qū)動飛行器飛行的螺旋槳是順時針，也就是往飛行器右滾的方向旋轉(zhuǎn)的。根據(jù)物理學(xué)角動量守恒定律，左右滾轉(zhuǎn)時必然會存在一個方向靈敏，一個方向不靈敏的情況。為了克服這種情況對姿態(tài)調(diào)整所造成的誤差，我們對PID算法進行了調(diào)定，令飛行器在往左滾和往右滾之時具有不一樣的PID系數(shù)，右滾的時候PID三環(huán)的系數(shù)設(shè)置比左滾的時候大，這樣就完美地解決了滾轉(zhuǎn)方向?qū)е碌撵`敏度差異問題。

3.2.3 飛行器自身的增穩(wěn)

除了導(dǎo)航外，飛行器在飛行過程中的整體穩(wěn)定性也是需要考慮的。為了增加整套飛行系統(tǒng)的魯棒性，筆者在其APM飛控中增加了飛行器在Pitch（俯仰）、Roll（滾轉(zhuǎn)）和Yaw（偏航）方向的角度限制[6]，當(dāng)飛行器超過這些角度限制時，會驅(qū)動副翼，使飛行器自動回正，以達到增加魯棒性的目的。同時，飛行器本身由于APM內(nèi)自帶的位置式PID，因此能保證了自身的飛行穩(wěn)定。APM與我們的激光導(dǎo)航系統(tǒng)融合計算，共同控制飛行器的導(dǎo)航制導(dǎo)飛行，保證飛行器在飛行上的穩(wěn)定和航向上的精度。

4 效果測試

激光光斑識別和飛行器飛行時的實際擊中效果如圖6所示，其中大的十字線是飛 中 軸線所在位置，小的十字線所在的位置是激光光斑所在的位置。經(jīng)過測試，這套方案能夠保證固定翼飛行器在30米的范圍內(nèi)能夠準確的擊中激光光斑所照射的位置。其精度半徑約達。

5 結(jié)論

本文探究了一種基于OpenMV的固定翼激光引導(dǎo)算法。經(jīng)過試驗證明，這套方案能夠在30m的范圍內(nèi)引導(dǎo)固定翼飛行器精確擊中目標(biāo)。為OpenMV在固定翼位置定位上的應(yīng)用以及視覺導(dǎo)航上面提供了實際應(yīng)用范例，探索了不依賴衛(wèi)星進行導(dǎo)航的一套可行的固定翼制導(dǎo)方案。

參考文獻：

[1]Zhen W， Zeng S， Soberer S. Robust localization and localizability estimation with a rotating laser scanner[C]//2017 IEEE International Conference on Robotics and Automation（ICRA）. IEEE， 2017：6240-6245.

[2]張兆龍.基于自抗擾控制的飛機飛行姿態(tài)控制研究[D].南京航空航天大學(xué)，2016.

[3]鄒浩，郭雨婷，李佳盈，等.基于OPENMV的色彩引導(dǎo)機器人系統(tǒng)研究[J].科技資訊，2018，16（25）：85-86.

[4]梅妍玭，傅榮.基于OpenMV的3D定位識別系統(tǒng)[J].新技術(shù)新工藝，2018（02）：50-52.

[5]韓鵬，耿增顯，劉宏，等.無人機視覺識別與移動追蹤技術(shù)綜述[J].飛航導(dǎo)彈，2018（10）：24-28.

[6]秦寧，胡立夫，耿家樂.基于可變數(shù)字目標(biāo)識別的四旋翼火災(zāi)監(jiān)測系統(tǒng)[J].中國科技信息，2019（01）：93-95