基于姿態(tài)解算和圖像識別算法的四旋翼自主飛行器系統(tǒng)

彭昕昀，熊正輝，黃偉強，何振環(huán)，歐森源，唐武翰，陳錦平

（韶關(guān)學(xué)院 智能工程學(xué)院，廣東 韶關(guān) 512005）

四旋翼自主飛行器系統(tǒng)具有普通飛行器的優(yōu)點，又具有無人機的造價低、可重復(fù)性強以及事故代價低等特點.在軍事上，可以用來進行地面戰(zhàn)場偵察和監(jiān)視，獲取情報.在民用上，可用于災(zāi)后搜救、城市交通巡邏和目標跟蹤等方面.在工業(yè)上，可用于安全巡檢，地震洪水、高壓輸電線等人工不容易到達空間進行安全任務(wù)檢查與搜救工作，能夠?qū)?zhí)行區(qū)域進行航拍和成圖等，四旋翼自主飛行器系統(tǒng)具有十分廣闊的應(yīng)用前景.

1 系統(tǒng)總體設(shè)計

四旋翼自主飛行器系統(tǒng)采用STM32F4 系列作為飛控板，內(nèi)嵌陀螺儀、加速度計和氣壓計；采用四路分電板經(jīng)四路無刷電調(diào)降壓供電，搭載OpenMV、Radio 數(shù)傳模塊、GPS 模塊、FS-iA10B 接收模塊；用富斯i6遙控器進行遙控，上位機使用MP 地面站，進行相關(guān)數(shù)據(jù)收發(fā).電池通過四路分電板分別給電調(diào)供電，經(jīng)電調(diào)的BEC 降壓接口給飛控板供電，電調(diào)的三相輸出口接三相電機，各個模塊統(tǒng)一由飛控板5 V 供電，當MCU 處理好模塊之間傳輸回來的數(shù)據(jù)時，會給電調(diào)輸出一個持續(xù)的PWM 信號，從而使電機實現(xiàn)持續(xù)可調(diào)節(jié)的轉(zhuǎn)動.系統(tǒng)的總體設(shè)計框圖如圖1 所示.

圖1 系統(tǒng)總體設(shè)計框圖

2 算法分析

2.1 姿態(tài)解算算法

姿態(tài)算法是利用四元數(shù)在三維空間內(nèi)的向量表示，來反映出物體在三維空間內(nèi)的姿態(tài)變換.利用歐拉角等效，求出物體實際姿態(tài)坐標和地理坐標之間的偏差，進行校正［1］.

四元數(shù)可以描述物體在三維空間內(nèi)的旋轉(zhuǎn)信息，可以利用四元數(shù)來推導(dǎo)出四元數(shù)在三維空間里的姿態(tài)矩陣.如式（1）所示，其中分別為三維空間內(nèi)單位向量的分量）［2］.

物體在三維空間內(nèi)的姿態(tài)變換，可以等效為物體繞X-Y-Z 軸的復(fù)合旋轉(zhuǎn)，對此可以用歐拉角在三維空間姿態(tài)，推導(dǎo)出變換矩陣公式.如式（2）所示，其中該公式的旋轉(zhuǎn)順序為Z-Y-X，繞Z 軸旋轉(zhuǎn)時產(chǎn)生的角度稱航向角（ψ），繞Y 軸旋轉(zhuǎn)時產(chǎn)生的角度稱俯仰角（γ），繞X 軸旋轉(zhuǎn)時產(chǎn)生的叫稱翻滾角（θ）［3］.

最后得出姿態(tài)角，如式（3）所示.

系統(tǒng)采用STM32 的HAL 庫和Python 進行編程.讓系統(tǒng)進入姿態(tài)模式，需要對姿態(tài)角信息進行采集，通過飛行系統(tǒng)上的陀螺儀獲取相應(yīng)的角速度，以及加速度計獲取的加速度，將這些數(shù)據(jù)繼續(xù)四元數(shù)微分求解，得到的數(shù)值進行歸一化，有利于求解相應(yīng)的歐拉角，進而可以得到無人機此時的姿態(tài)角，將其與地理坐標系角度比較求差，求出偏離地理坐標系角度，再通過PID 處理，將結(jié)果返回給飛行系統(tǒng)的MCU，進而做出姿態(tài)修正.姿態(tài)解算流程圖如圖2 所示［4］.

圖2 姿態(tài)解算流程圖

2.2 圖像處理算法

2.2.1 中值濾波

為了提高圖像的質(zhì)量、抑制噪聲，目標的識別前需要對采集的每一幀圖像做濾波處理，本系統(tǒng)采用中值濾波技術(shù).中值濾波是基于排序統(tǒng)計理論原理的一種能有效抑制噪聲的非線性信號處理技術(shù).它是一種鄰域運算，把數(shù)字圖像或數(shù)字序列中突出點的值用鄰域中各點值得中值代替，使周圍像素灰度值比較大的像素趨向周圍像素值，從而可以消除孤立的噪點.二維中值濾波輸出，如式（4）所示［5］.

式中，f（x，y），g（x，y）分別為原圖像和處理后圖像，W 為二維模板，通常為3*3.

對圖3 中的各灰度值進行排序，取中值130 替換255，得到圖4 的灰度值數(shù)據(jù)分布，可有效去除噪點.

圖3 模板灰度值分布圖

圖4 中值濾波后灰度值分布圖

2.2.2 MeanShift 圖像識別算法

系統(tǒng)采用MeanShift 圖像識別算法進行目標識別，把采集的圖像轉(zhuǎn)換到RGB 空間形成顏色直方圖，再用MeanShift 算法匹配特征值并進行標記，最終實現(xiàn)對目標的識別跟蹤.在實際識別目標跟蹤時，運動目標在運動過程中顏色信息變化不大，光線等外在因素影響較小，使MeanShift 算法具有了良好的魯棒性.MeanShift 算法思想是假設(shè)不同蔟類的數(shù)據(jù)集符合不同的概率分布，找到任一樣點密度增大的最快方向，樣本密度高的區(qū)域?qū)?yīng)于該分布的最大值，這些樣本點最終會在局部密度最大值收斂.在充分采樣的情況下，可以逐漸收斂于任意的密度函數(shù)，滿足任意分布的數(shù)據(jù)密度估計［6］.

MeanShift 算法的基本思想及物理意義：

給定d 維空間Rd中的n 個樣本點xi，其中i=1，…，n，在x 點的MeanShift 向量的基本形式定義為：

其中，Sh是一個半徑為h 的高維球區(qū)域，滿足以下關(guān)系的y 點的集合為：

h 表示在這n 個樣本點xi中有h 個點落入Sh區(qū)域中，式（5）中（xi-x）是樣本點xi點相對于點x 的偏移向量.原理由圖5 所示.

圖5 MeanShift 算法原理圖

由圖可知，大圓圈所圈定的范圍為Sh，也就是目標特征點分布區(qū)域，小圓圈代表落入Sh區(qū)域內(nèi)的樣本點xi∈Sh，黑點就是MeanShift 的基準點x，箭頭表示樣本點相對于基準點x 的偏移向量.在實際的目標識別跟蹤中，平均的偏移向量Mh（x）會指向樣本分布最多的地方，也就是概率密度函數(shù)的梯度方向，實現(xiàn)飛行器始終趨向目標進行移動跟蹤［7-8］.

3 硬件設(shè)計

3.1 攝像頭跟蹤模塊

OpenMV 是一款以 STM32 為處理核心的嵌入式攝像頭，本系統(tǒng)采用OpenMV 進行一系列的圖像采集和處理任務(wù)，對目標物體進行圖像處理及實時跟蹤起到重要的作用. OpenMV 攝像頭模塊如圖6 所示.

3.2 通信系統(tǒng)

飛行器系統(tǒng)采用的通信有無線電通信、跳頻通信、串口通信.通過Radio 數(shù)傳模塊和上位機進行無線電數(shù)據(jù)收發(fā)；富斯i6 遙控器通過跳頻技術(shù)通信，對搭載在飛行系統(tǒng)上的接收器進行信號的收發(fā). Radio 數(shù)傳模塊見圖7，富斯i6 接收器見圖8.

圖6 Openmv 攝像頭模塊

圖7 Radio 數(shù)傳模塊

圖8 富斯i6 接收器

3.3 輔助電源

硬件系統(tǒng)搭載4 個功率為120 W 的新西達無刷電機，使用12 V 的電池經(jīng)4 路分電板進行同步供電；同時為滿足飛行系統(tǒng)上的各種模塊供電，系統(tǒng)搭載4 路無刷電調(diào)進行降壓，給飛行器控制器、數(shù)據(jù)傳輸、接收器等各個模塊進行供電.

3.4 異常報警和保護

為了防止在飛行的過程出現(xiàn)數(shù)據(jù)包大面積丟失造成飛行事故，加入LED 燈和蜂鳴器.當出現(xiàn)上電自檢不正常、飛行過程電池電壓過低時，會通過LED 燈閃爍方式和蜂鳴器鳴叫發(fā)出警告.當出現(xiàn)數(shù)據(jù)包大面積丟失時，會發(fā)出蜂鳴警告，同時飛行系統(tǒng)會進行定速迫降.

3.5 硬件系統(tǒng)實現(xiàn)

系統(tǒng)采用S500 機架為載體，搭載STM32 主控板，OpenMV 攝像頭模塊，經(jīng)過安裝制作，系統(tǒng)的硬件實物見圖9.

4 軟件設(shè)計

四旋翼自主飛行器系統(tǒng)是一個多通道的飛行控制系統(tǒng)，軟件系統(tǒng)的主程序先進行系統(tǒng)初始化，之后進入自檢狀態(tài)，然后進入鎖定模式，再通過手動解鎖啟動飛行，此時可根據(jù)遙控器進行姿態(tài)切換，如果飛行器出現(xiàn)了低壓報警，則進行手動迫降.系統(tǒng)的主程序流程如圖10 所示［9］.

系統(tǒng)啟動飛行后，系統(tǒng)進入姿態(tài)模式.該模式下，可根據(jù)遙控器的通道選擇，令飛行器進入追蹤模式，飛行器上搭載的OpenMV 模塊會初始化采集圖像，并進行中值濾波，微處理器根據(jù)MeanShift 算法進行偏差計算，得到的偏差經(jīng)過互補濾波，最后由系統(tǒng)進行校正.在姿態(tài)飛行模式下，系統(tǒng)會讀取陀螺儀和加速度計的數(shù)據(jù)，然后進行數(shù)據(jù)積分，最后計算偏差，經(jīng)過濾波后由系統(tǒng)開始校正［10-11］. 飛行器系統(tǒng)的姿態(tài)切換流程圖如圖11 所示.

圖9 系統(tǒng)硬件實物圖

圖10 四旋翼自主飛行器系統(tǒng)主程序流程圖

在追蹤模式下，飛行器系統(tǒng)搭載的OpenMV 模塊完成圖像采集，通過微處理器對采集到的圖像做處理、目標檢測、目標跟蹤等［12］，利用PID算法由微處理器輸出PWM 方波，以此控制電機，跟蹤模式示意圖如圖12 所示.

圖11 姿態(tài)切換子程序流程圖

圖12 跟蹤模式示意圖

5 系統(tǒng)調(diào)試和測試

通過上位機將飛行數(shù)據(jù)配置好之后，設(shè)定遙控器通道上對應(yīng)的飛行模式，即可對飛行器進行解鎖飛行操作.上位機界面圖如圖13 所示.

圖13 上位機界面圖

通過飛行測試，飛行器可以穩(wěn)定的上升、下降、翻滾、偏航.數(shù)據(jù)傳輸最大距離為800 m，遙控器接收最大距離為700 m.電池采用11.1 V 容量為3 700 mah 的鋰電池，經(jīng)測試電池使用大約7 min 就會低壓報警.可通過上位機接收飛行器的姿態(tài)數(shù)據(jù)，其中ACC_X 為X 軸旋轉(zhuǎn)角度，ACC_Y 為Y 軸旋轉(zhuǎn)角度，ACC_Z 為Z 軸旋轉(zhuǎn)角度.上位機接收飛行器的姿態(tài)數(shù)據(jù)圖如圖14 所示.

圖14 上位機接收數(shù)據(jù)波形圖

在圖像識別中，以小球作為目標，進行識別跟蹤測試.微處理器對采集的圖像做RGB 三種顏色直方圖轉(zhuǎn)換.采用十字可標定目標中心，方框框住目標，方框與十字標隨著目標的移動而移動，實現(xiàn)對目標的跟蹤.跟蹤效果如圖15 所示.

6 結(jié)語

系統(tǒng)通過攝像頭獲取圖像，計算出相關(guān)偏差數(shù)據(jù)，進行PID 校正.結(jié)合上位機遠程監(jiān)測相關(guān)飛行數(shù)據(jù).采用四元數(shù)姿態(tài)解算算法，實現(xiàn)飛行器自穩(wěn)狀態(tài)下飛行；采用濾波算法和MeanShift 圖像識別算法，實現(xiàn)顏色識別.

通過測試，系統(tǒng)實現(xiàn)了四旋翼飛行器的姿態(tài)飛行，可進行翻滾動作、俯仰動作，偏航動作、顏色識別和目標追蹤等功能，系統(tǒng)飛行狀況穩(wěn)定良好，目標跟蹤識別準確，具有良好的應(yīng)用前景.

圖15 目標跟蹤效果圖