基于姿態解算和圖像識別算法的四旋翼自主飛行器系統

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

（韶關學院 智能工程學院，廣東 韶關 512005）

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

1 系統總體設計

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

圖1 系統總體設計框圖

2 算法分析

2.1 姿態解算算法

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

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

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

最后得出姿態角，如式（3）所示.

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

圖2 姿態解算流程圖

2.2 圖像處理算法

2.2.1 中值濾波

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

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

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

圖3 模板灰度值分布圖

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

2.2.2 MeanShift 圖像識別算法

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

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

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

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

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

圖5 MeanShift 算法原理圖

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

3 硬件設計

3.1 攝像頭跟蹤模塊

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

3.2 通信系統

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

圖6 Openmv 攝像頭模塊

圖7 Radio 數傳模塊

圖8 富斯i6 接收器

3.3 輔助電源

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

3.4 異常報警和保護

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

3.5 硬件系統實現

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

4 軟件設計

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

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

圖9 系統硬件實物圖

圖10 四旋翼自主飛行器系統主程序流程圖

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

圖11 姿態切換子程序流程圖

圖12 跟蹤模式示意圖

5 系統調試和測試

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

圖13 上位機界面圖

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

圖14 上位機接收數據波形圖

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

6 結語

系統通過攝像頭獲取圖像，計算出相關偏差數據，進行PID 校正.結合上位機遠程監測相關飛行數據.采用四元數姿態解算算法，實現飛行器自穩狀態下飛行；采用濾波算法和MeanShift 圖像識別算法，實現顏色識別.

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

圖15 目標跟蹤效果圖