一種基于OpenMV的飛行器激光引導算法

黃迅 崔敏 黃隆群 邱敏卿

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

關鍵詞：固定翼;定位算法;激光引導;OpenMV;PID

中圖分類號：TP242? ? ? ? ?文獻標志碼：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 概述

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

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

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

2 激光引導系統的總體架構

在本例中，激光引導系統主要由紅色激光照射器、固定翼飛行器、APM開源飛控、姿態調整執行器和OpenMV開源圖像識別模塊等五部分組成。

2.1 紅色激光照射器

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

2.2 固定翼飛行器

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

2.3 APM開源飛控

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

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

2.4 姿態調整執行器

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

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

2.5 OpenMV開源圖像識別模塊

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

3 設計與實驗

3.1 激光信號識別

3.1.1 OpenMV程序的基本結構形式

在本應用中，OpenMV的作用就是要識別達到目標物上的激光。由于筆者使用的激光是一種紅色的可見激光，因此可以通過OpenMV中自帶的查找色塊函數直接找出環境中的激光。

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

即所有的程序都放在不斷的循環之中，以循環的方式進行相應的處理。

3.1.2 激光點識別算法

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

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

在程序實現上，筆者采用了OpenMV中自帶的查找視野中對應LAB色域色塊的函數：

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

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

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

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

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

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

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

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

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

3.2 飛行器導航算法

3.2.1 激光制導思路

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

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

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

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

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

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

在最初的偏差量進入了PID系統，并得到最終的調定輸出量之后，其輸出量還需要轉化為OpenMV控制舵機所能接受的角度數值，如下面的代碼所示：

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

3.2.2 滾轉方向的靈敏度

在實際開發過程中，還發現滾轉方向姿態控制具有左滾轉和右滾轉靈敏度不同的問題，通常，飛行器左滾的趨勢會比右滾更加明顯。經過仔細分析之后發現，之所以靈敏度會不同，是因為驅動飛行器飛行的螺旋槳是順時針，也就是往飛行器右滾的方向旋轉的。根據物理學角動量守恒定律，左右滾轉時必然會存在一個方向靈敏，一個方向不靈敏的情況。為了克服這種情況對姿態調整所造成的誤差，我們對PID算法進行了調定，令飛行器在往左滾和往右滾之時具有不一樣的PID系數，右滾的時候PID三環的系數設置比左滾的時候大，這樣就完美地解決了滾轉方向導致的靈敏度差異問題。

3.2.3 飛行器自身的增穩

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

4 效果測試

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

5 結論

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

參考文獻：

[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]張兆龍.基于自抗擾控制的飛機飛行姿態控制研究[D].南京航空航天大學，2016.

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

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

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

[6]秦寧，胡立夫，耿家樂.基于可變數字目標識別的四旋翼火災監測系統[J].中國科技信息，2019（01）：93-95