低成本四旋翼高清圖像采集功能的研究與實現(xiàn)

馬正華，孫 斌，顧蘇杭

(常州大學 信息科學與工程學院，江蘇 常州 213100)

0 引 言

目前四旋翼飛行器的研究和應用領域已經(jīng)十分廣泛，而關于四旋翼飛行器的設計和研發(fā)，也涌現(xiàn)出了一大批成熟有效的設計方案。如利用DSP高速數(shù)字處理器對四旋翼硬件平臺進行研發(fā)和設計的方案[1]、利用ARM微處理器實現(xiàn)四旋翼飛行器的電視引導察打一體化的設計方案[2]。在這些設計方案中，由于預算的原因，某些方案不得不采用低成本的四旋翼飛行器。然而，很多低成本的四軸飛行器難以在脫離遙控的情況下自穩(wěn)飛行，部分能自穩(wěn)飛行的四軸飛行器又不能拍攝高清圖片，成本已然成為這類四旋翼飛行器發(fā)展的瓶頸。為解決這個問題，使低成本四旋翼飛行器能夠做到在脫離遙控定高自穩(wěn)飛行的同時拍攝500萬級像素的高清圖片，設計了一種以低成本四旋翼為載體的高清圖像采集系統(tǒng)。所設計的四旋翼飛行器以中低端單片機STM32為控制核心，并利用串級PID及時間片輪式控制算法控制飛行器的姿態(tài)，使之具備脫控自穩(wěn)飛行的能力；在飛行器達到穩(wěn)定的飛行狀態(tài)時，開啟圖像采集模塊拍攝圖片，該模塊采用了高清攝像頭+SDRAM+CPLD的緩存模式[3,4]，可在不影響控制中心運算速度和存儲中心運行效率的情況下采集高清圖像，并以無線的方式傳輸給PC機。從而實現(xiàn)了低成本四旋翼飛行器依然可以完成高精度自穩(wěn)控制、高清采圖等任務的效果。

1 四旋翼飛行器硬件總體設計

該四旋翼飛控系統(tǒng)采用ST意法半導體公司的中低端單片機STM32F407VGT6作為控制中心。該芯片最高主頻可達168 Mhz,具有192 KB的SRAM、1024 KB的FLASH，自帶12個16位定時器，并且配備3個SPI、3個IIC、6個串口、2個USB(HOST/SLAVE)等總線，擴展性強[5]，可支持飛行器完成絕大部分的飛行任務。

地磁傳感器AK8975以及MPU6050共同完成四旋翼的9軸姿態(tài)結算任務。MPU6050通過其第二IIC接口與AK8975相連接。MPU6050內(nèi)置了一個數(shù)字運動處理器DMP，其可根據(jù)InvenSen公司提供的運動處理資料包，方便的將9軸姿態(tài)數(shù)據(jù)解算出來。

MS5611高度氣壓計和超聲波模塊相結合，可使四軸飛行器實現(xiàn)定高的功能。在實際測試過程中，發(fā)現(xiàn)超聲波模塊由于其自身的缺陷，在飛行高度超過地面3 m后因噪聲增大而影響性能。此時就應用MS5611氣壓計代替超聲波實現(xiàn)定高的功能。

四旋翼通過四路無刷直流電機控制其姿態(tài)的變換。在預算有限的情況下，為保證飛行器動力充足，實驗平臺選用了朗宇的2212 KV800無刷直流電機，并選用最大輸出20A的電調(diào)作為電機與MCU的溝通橋梁。

為實現(xiàn)無線傳輸?shù)墓δ埽捎肅C2530無線透傳模塊[6]。由于高清圖像采集模塊的輸出接口為串口，而與PC機溝通的模塊為USB轉(zhuǎn)TTL，故采用CC2530無線串口模塊。該模塊可實現(xiàn)帶確認的點對點傳輸，數(shù)據(jù)丟包率為0.00%，而且支持串口不間斷發(fā)送，適合本四旋翼飛行器的應用場景。四旋翼具體結構如圖1所示。由圖1可知，實驗平臺所用功能模塊及主控MCU均不是高成本器件，機架、電機、電調(diào)、飛控板等重要部分總成本不超過300元人民幣，但其組成的四旋翼飛行器卻可以實現(xiàn)高精度自穩(wěn)及高清采圖這些昂貴飛行器才能實現(xiàn)的功能。

2 四旋翼飛行器控制算法設計

2.1 四旋翼飛行器建模分析

要使低成本四旋翼飛行器完成采集高清圖片的任務，前提是飛行器可以達到高精度脫控自穩(wěn)的飛行狀態(tài)。要實現(xiàn)飛行器的自穩(wěn)控制，需要分析其動力學模型，簡化模型如圖2所示。

圖1 四旋翼飛行器硬件總體設計結構

圖2 四旋翼飛行器簡化模型

(1)

式中：U1為飛行器上升的總升力控制量，U2代表橫滾姿態(tài)φ以及y方向上的運動控制量，U3代表俯仰姿態(tài)θ以及x軸方向的控制量，U4代表航偏姿態(tài)ψ。

由質(zhì)心運動定理和歐拉運動方程，可建立機體坐標系下x、y、z軸方向上合外力方程以及繞x、y、z軸的合外力矩方程

(2)

式中：∑Fx，∑Fy，∑Fz分別為機體坐標系下各軸方向上的合外力；∑Mx，∑My，∑Mz為軸上繞三軸的合外力矩[8]。u，v，w分別為機體坐標系下x，y，z軸上的線速度。p，q，r分別為機體坐標系下x，y，z軸轉(zhuǎn)動的角速度。Ix，Iy，Iz分別為3個軸方向上的慣性主矩。

可通過矩陣R來獲得機體坐標系到地理坐標系的轉(zhuǎn)換

(3)

式中：C和S分別代表三角函數(shù)Cos和Sin。由轉(zhuǎn)移矩陣R可推算出四旋翼飛行器對應地坐標系的非線性方程

(4)

式中：Rij表示轉(zhuǎn)移矩陣R中對應第i行第j列的元素。Fx，F(xiàn)y，F(xiàn)z分別表示四軸飛行器體坐標系下所受合外力在各軸方向的分量，m表示四旋翼的質(zhì)量。姿態(tài)角與機體坐標系下角速度的關系如方程(5)所示

(5)

由方程(5)可進一步整理為

(6)

由式(6)可知，四旋翼飛行器最終的飛行狀態(tài)和姿態(tài)角加速度以及線加速度有關。單級PID控制系統(tǒng)不能夠同時處理位置、速度、加速度等因子，因此，必須采用串級PID來控制飛行器的飛行狀態(tài)。

2.2 四旋翼串級PID控制算法設計

由2.1分析可知，要實現(xiàn)四旋翼姿態(tài)的自穩(wěn)控制功能，須采用串級PID來控制飛行器的飛行狀態(tài)[9,10]。這里以3個歐拉角(橫滾角ROLL、俯仰角PIT、航向角YAW)的串級PID控制系統(tǒng)以及定高控制的PID控制系統(tǒng)為例。

圖3 姿態(tài)角串級PID

四旋翼平穩(wěn)飛行的另一要求是定高，而定高功能的實現(xiàn)是以自穩(wěn)為基礎的。因此在為四旋翼建立自穩(wěn)功能串級PID控制系統(tǒng)后，亦建立了串級PID高度控制系統(tǒng)。

圖4 高度控制串級PID

2.3 四旋翼時間片輪式任務調(diào)度

四旋翼飛行器的飛控程序由非常多的任務組成，而每個任務的執(zhí)行周期均不同，因此只有把多個任務規(guī)劃好才能實現(xiàn)飛控系統(tǒng)的有序運行。

在普通的四軸飛行器控制算法中，所有的任務都在一個循環(huán)中運行，任務的優(yōu)先級由對應中斷的優(yōu)先級控制。該種控制算法雖然也能使四旋翼完成基本的飛行任務，但是在其飛行過程中，始終需要遙控器對其飛行姿態(tài)進行調(diào)整；更甚者，若中斷設置不合理，還可能造成因任務沖突而引起的摔機事故。因此，需要對飛行器的控制算法進行改進。

在改進版飛控程序的設計過程中，應嚴格控制代碼執(zhí)行的順序和時間，不能隨意進行軟件延時，以保證系統(tǒng)的效率；不能隨意采用多個優(yōu)先級不同的中斷，以保證程序的執(zhí)行順序足夠可靠；重要的任務需要統(tǒng)計每兩次相鄰調(diào)用的時間間隔，以保證系統(tǒng)的穩(wěn)定性。因此，在整個程序執(zhí)行過程中，僅開啟Stm32自帶的高優(yōu)先級Systick定時器，并通過時間片輪的方式執(zhí)行多個任務[11]。Systick定時器又稱滴答定時器，每1 ms產(chǎn)生一個中斷，而每個周期都有對應的標志位，Systick定時器每產(chǎn)生一個中斷，周期的標志位加一。在主循環(huán)中，每一個任務都有自己的執(zhí)行周期，控制器通過比較執(zhí)行周期的標志位數(shù)值來執(zhí)行相應的任務。當周期標志位數(shù)值達到其對應的值時，任務開始，任務開始前，周期標志位清零并開始自增；任務結束后，標志位再次清零開始下一個任務循環(huán)。采用該時間片輪的方式在執(zhí)行多個任務的同時還可以統(tǒng)計重要任務每兩次相鄰執(zhí)行的時間間隔，從而對重要任務是否正常執(zhí)行做出判斷，故系統(tǒng)的穩(wěn)定性也能得到保障。具體時間片輪式任務調(diào)度機制如圖5所示。

圖5 時間片輪式任務調(diào)度機制

至此，四旋翼飛行器的控制算法的改進工作完成。該低成本四軸飛行器已經(jīng)能夠完成自穩(wěn)飛行和定高的任務。

3 四旋翼飛行器高清圖像采集系統(tǒng)

所設計的四旋翼飛行器采用STM32F407ZGT6作為其主控制器。該款芯片的SRAM為192 KB，自帶的FLASH也達到了1024 KB，相對于其它的單片機來說，STM32F407的存儲容量已很大。但是，該低成本四旋翼需要拍攝至少500萬像素級的圖片，而500萬像素級的高清圖片即使壓縮為JPEG格式，其大小也有600 KB～2 MB。因此，僅靠STM32F407的SRAM和FLASH是無法完成拍攝任務的，更何況FLASH的讀取速度很慢，會嚴重影響MCU的運行效率。為解決這個問題，設計了一款基于高清攝像頭+SDRAM+CPLD模式的圖像采集系統(tǒng)。該圖像采集系統(tǒng)可以繞開拍攝高清圖片所需要的內(nèi)存空間和處理速度，使中低端單片機可以像高端的微處理器一樣，拍攝500萬像素級甚至800萬像素級的高清圖片。

3.1 高清圖像采集系統(tǒng)硬件結構

如圖6所示，虛線框中為圖像采集系統(tǒng)的硬件結構圖。可知該圖像采集系統(tǒng)主要由高清攝像頭OV5640、高速處理器CPLD、大容量存儲器SDRAM三部分組成。若對圖像有更高的要求，可以用800萬像素的OV8865攝像頭替換OV5640，這里以OV5640為例說明。

在圖像采集系統(tǒng)中，由OV5640完成采集圖像的任務。OV5640是OmniVision公司設計的一款分辨率為5 MP的攝像頭。其感光面積為1/4英寸，像素大小為1.4 μm*1.4 μm，且內(nèi)置自動對焦馬達，可對焦的范圍為5 cm～無限遠。其內(nèi)置的JPEG壓縮引擎，可直接輸出高質(zhì)量的JPEG格式的圖像數(shù)據(jù)。該攝像頭極大簡化了超清拍攝的應用開發(fā)過程。

EPM240T100是由Altera公司推出的一款具有革命性意義的CPLD產(chǎn)品。它基于全新的CPLD架構，幾乎囊括CPLD全系列產(chǎn)品的IO管腳。并且EPM240T100采用全新的查表體系(LUT)，在降低成本和功耗的同時，保留上一代MAX系列原有的瞬時啟動、單芯片結構、非易失性和易用性等優(yōu)勢，具有革命性意義。EPM240T100在本圖像采集系統(tǒng)中，作為傳遞數(shù)據(jù)信息的橋梁，協(xié)調(diào)OV5640攝像頭、SDRAM以及主控MCU完成數(shù)據(jù)傳遞的工作。

W9864G6KH是由Winbond公司推出一款8M Byte的高速SDRAM。其具有功耗低、容量大、成本低的優(yōu)點。在本圖像采集系統(tǒng)中，當OV5640攝像頭采集到圖像后，便將數(shù)據(jù)直接存儲在該SDRAM中，等待CPLD將其數(shù)據(jù)發(fā)送給主控MCU的數(shù)據(jù)接口。W9864G6KH在工作過程中，總是需要外部設備對其進行定時性刷新充電，在此圖像采集系統(tǒng)中，它的刷新和讀寫都是由CPLD完成的。

圖6 高清圖像采集系統(tǒng)硬件結構

3.2 高清圖像采集系統(tǒng)工作過程

當圖像采集系統(tǒng)上電初始化后，PC機發(fā)送一個“握手”指令，該指令通過CC2530模塊以無線的方式被傳送至四旋翼飛行器。四旋翼飛控接到指令，再次以無線的方式將“確認”指令傳送給PC機，地面站PC機和四旋翼配對成功。

此時，若PC機發(fā)送“拍照”的指令，該指令將會通過CC2530無線串口模塊再次傳送給四旋翼飛行器。四旋翼主控MCU接到“拍照”指令后，通過I2C總線將該指令傳入OV5640高清攝像頭，OV5640執(zhí)行拍照的操作。由于OV5640自帶JPEG圖像壓縮引擎，其在采集到圖像后直接輸出JPEG格式的圖像數(shù)據(jù)。該圖像數(shù)據(jù)經(jīng)過CPLD內(nèi)置的數(shù)據(jù)總線仲裁器存儲在SDRAM中。當圖像數(shù)據(jù)存儲完畢后，主控MCU利用自帶的SPI總線通過CPLD的“24位地址及圖片長度寄存器”以及“SPI移位輸出寄存器”一段一段(程序中設定，每一段為512字節(jié))的將圖像數(shù)據(jù)從SDRAM中搬運至MCU的“SPI圖像數(shù)據(jù)接收單元”。被搬運出來的圖像數(shù)據(jù)再經(jīng)主控MCU內(nèi)部的“UART指令及數(shù)據(jù)接口”傳送給CC2530無線模塊，最后以無線的方式傳送給地面站PC機。通過PC機的上位機，可將圖片顯示出來。

4 實驗驗證

由于設計的高清圖像采集系統(tǒng)是以低成本四旋翼為載體的，故實驗驗證分為兩部分。第一部分是驗證四旋翼飛行狀態(tài)的穩(wěn)定性，第二部分是驗證圖像采集系統(tǒng)采集到圖像的質(zhì)量。

4.1 驗證四旋翼飛行狀態(tài)的穩(wěn)定性

四旋翼飛行過程中會受到的很多影響因素影響，如：重力、陀螺效應、空氣動力等。因此在實際測試中，很多硬件參數(shù)需要標定，本四旋翼的相關參數(shù)如下：l=25cm，m=1.25kg，Ix=0.0202kg·m2，Iy=0.0201kg·m2，Iz=0.0404kg·m2。

由2.2中的圖3可知，本設計采用串級PID來控制四旋翼的飛行姿態(tài)，而在該串級PID控制系統(tǒng)中，每一級PID輸出量的位置式表達式為

(7)

因此，測試和標定姿態(tài)角的比例系數(shù)KP、積分系數(shù)KI、微分系數(shù)KD則至關重要。經(jīng)過多次實驗和調(diào)試，最終確定3個姿態(tài)角的PID參數(shù)見表1，在該PID參數(shù)下，四旋翼可以達到平穩(wěn)的飛行效果。

表1 姿態(tài)角PID參數(shù)

當四旋翼的PID的參數(shù)整定完畢后，對四旋翼低空自穩(wěn)飛行時的姿態(tài)角進行實時監(jiān)控，姿態(tài)角的具體變化如圖7所示。從圖中可以看出，滾轉(zhuǎn)角和俯仰角的變化均在-5°～5°之間，而航偏角的變化均在-30°～-15°之間，充分證明了控制算法的有效性和穩(wěn)定性。

圖7 四旋翼自穩(wěn)狀態(tài)下姿態(tài)角曲線

4.2 高清圖像采集系統(tǒng)的測試

本文為該款低成本四旋翼設計的圖像采集系統(tǒng)，采用高清攝像頭OV5640+CPLD+SDRAM的緩存模式。攝像頭采集到的圖片數(shù)據(jù)首先存入SDRAM中，再利用主控MCU一段一段的將圖像數(shù)據(jù)發(fā)送出去。采用該種模式的圖像采集系統(tǒng)，可幫助中低端MCU繞開拍攝高清圖片所需要的處理速度和存儲空間，而只需像驅(qū)動低速設備一樣即可實現(xiàn)高清圖像拍攝。

如圖8所示，在無風的室外環(huán)境下對四旋翼進行測試。在低空狀態(tài)下，在四旋翼達到穩(wěn)定飛行狀態(tài)后，開啟圖像采集系統(tǒng)拍攝圖像。

圖8 四旋翼室外飛行狀態(tài)測試

為了對比本圖像采集系統(tǒng)的效果，使用目前市面上普通的低成本四旋翼與本設計做比較。兩個四旋翼先后在大致同一位置，開啟圖像采集系統(tǒng)。如圖9所示，左圖為本圖像采集系統(tǒng)拍攝的高清圖片，右圖為市面上低成本四旋翼自帶圖像采集系統(tǒng)拍攝的圖片。由圖可知，本圖像采集系統(tǒng)拍攝的圖片效果遠遠超過普通低成本四旋翼的圖像采集系統(tǒng)。

5 結束語

在四旋翼的研究和應用領域越來越廣泛、四旋翼的需求量越來越大的情況下，一部分低成本四旋翼很難做到在高精度自穩(wěn)控制的同時采集高清圖片。本文針對這類低成本四旋翼飛行器的缺陷，對其飛行控制算法和圖像采集系統(tǒng)進行了改進。在飛控算法中，采用多個串級PID來控制四旋翼的飛行姿態(tài)和高度；同時采用時間片輪的方式協(xié)助MCU進行多個任務的調(diào)度，極大了保證了四旋翼飛行的穩(wěn)定性，使低成本四旋翼亦可實現(xiàn)高精度的自穩(wěn)控制。在圖像采集方面，本文借鑒文獻[3,4]的設計思路及相關的實際工程項目，采用高清攝頭+SDRAM+CPLD的緩存模式改進飛行器的圖像采集系統(tǒng)，使中低端的MCU依然可以實現(xiàn)高清圖片的拍攝效果。最終，低成本四旋翼飛行器可以實現(xiàn)在高精度自穩(wěn)控制的同時拍攝高清圖片。

參考文獻：

[1]JI Yidao,PAN Xun,GUO Yukun,et al.Hardware platform design and realization of quadrotor UAV system[J].Computer Engineering and Design,2016,37(1):76-80(in Chinese).[吉一纛,潘汛,郭玉坤,等.四旋翼飛行器系統(tǒng)硬件平臺設計與實現(xiàn)[J].計算機工程與設計,2016,37(1):76-80.]

[2]CHEN Jie,WANG Risheng,JIANG Yufeng,et al.Design and realization of ARM-based fly task control system in UAV,guided by small TV with the ability of attacking and scouting[J].Science and Technology Review,2013,31(1):27-32(in Chinese).[陳杰,王日勝,蔣玉峰,等.基于ARM的小型電視引導察打一體無人機任務控制系統(tǒng)設計與實現(xiàn)[J].科技導報,2013,31(1):27-32.]

[3]Bourrasset C,Maggianiy L,Sérot J,et al.Distributed FPGA-based smart camera architecture for computer vision applications[C]//International Conference on Distributed Smart Cameras.IEEE,2013:1-2.

[4]Bourrasset C,Serot J,Berry F.FPGA-based smart camera mote for pervasive wireless network[C]//Seventh International Confe-rence on Distributed Smart Cameras.IEEE,2013:1-6.

[5]ZHANG Yang,LIU Jun,YAN Hanyu.Proficient in Stm32:Library function version[M].Beijing:Beihang University Press,2015(in Chinese).[張洋,劉軍,嚴漢宇.精通STM32F4:庫函數(shù)版[M].北京:北京航空航天大學出版社,2015.]

[6]YANG Song,HU Guorong,XU Peicheng.Design and implementation of MAC layer in ZigBee based on CC2530[J].Computer Engineering and Design,2013,34(11):3840-3844(in Chinese).[楊松,胡國榮,徐沛成.基于CC2530的ZigBee協(xié)議MAC層設計與實現(xiàn)[J].計算機工程與設計,2013,34(11):3840-3844.]

[7]MA Zhenghua,CHEN Jie,CHEN Lanping,et al.Gyroscope achieves the four rotor UAV attitude stabilization control[J].Computer Simulation,2016,33(10):32-38(in Chinese).[馬正華,陳杰,陳嵐萍,等.四旋翼無人機陀螺儀實現(xiàn)姿態(tài)穩(wěn)定性控制[J].計算機仿真,2016,33(10):32-38.]

[8]LIAO Yihua,ZHANG Tiemin,LAN Yubin.Design and test of attitude stabilization control system of multi-rotor unmanned aerial vehicle applied in farmland information acquisition[J].Transactions of the Chinese Society of Agricultural Engineering,2017,33(3):88-98(in Chinese).[廖懿華,張鐵民,蘭玉彬.農(nóng)田信息采集用多旋翼無人機姿態(tài)穩(wěn)定控制系統(tǒng)設計與試驗[J].農(nóng)業(yè)工程學報,2017,33(3):88-98.]

[9]TAN Guangchao.Design and implementation of attitude control system for a quadrotor UAV[D].Dalian:Dalian University of Technology,2013:25-29(in Chinese).[譚廣超.四旋翼飛行器姿態(tài)控制系統(tǒng)的設計與實現(xiàn)[D].大連:大連理工大學,2013:25-29.]

[10]CHEN Qi.Research on advanced control method and realization of unmanned quadrotor helicopter[D].Nanjing:Nanjing University of Aeronautics and Astronautics,2013:15-16(in Chinese).[陳琦.四旋翼無人直升機先進飛行控制方法及實現(xiàn)研究[D].南京:南京航空航天大學,2013:15-16.]

[11]XIAO Zhu.Research of dispatching algorithm and reliability control in multitask flight control system[D].Chengdu:University of Electronic Science and Technology of China,2012:18-25(in Chinese).[肖柱.多任務飛行控制系統(tǒng)中調(diào)度算法與可靠性控制研究[D].成都:電子科技大學,2012:18-25.]