基于視覺引導(dǎo)的室內(nèi)四旋翼飛行器控制系統(tǒng)研究與設(shè)計

時夢瑤，李建奇，黃瑞敏，張 斌，田中杰

（湖南文理學(xué)院 電氣與信息工程學(xué)院，常德 415000）

四旋翼飛行器設(shè)計是一項包含多種交叉學(xué)科的尖端技術(shù)領(lǐng)域。隨著四旋翼飛行器在軍用、民用和商用領(lǐng)域的應(yīng)用范圍越來越廣，對四旋翼飛行器在導(dǎo)航系統(tǒng)和視覺定位方面也提出了新的要求，四旋翼飛行器中通過視覺循跡成為當(dāng)下熱門研究領(lǐng)域之一[1]?；谝曈X循跡是指通過圖像的目標(biāo)識別與跟蹤技術(shù)來獲得目標(biāo)的空間位置，當(dāng)四旋翼飛行器在低空飛行時，可通過裝載的攝像頭來獲得實時路徑信息，并通過相應(yīng)算法，使飛行器在無人干擾的情況下移動到目的地[2-4]。

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

整個飛行控制系統(tǒng)總體構(gòu)成，包括飛行器平臺、圖像處理系統(tǒng)、供電電池及電壓轉(zhuǎn)換模塊和無線遙控接收系統(tǒng)。系統(tǒng)的總體設(shè)計如圖1所示。

圖1 系統(tǒng)的總體設(shè)計Fig.1 Overall design of system

飛行器平臺包括3部分：①微處理器最小系統(tǒng)，用于處理數(shù)據(jù)和協(xié)調(diào)控制；②傳感器模塊，包括九軸姿態(tài)傳感器、高度傳感器，為飛控系統(tǒng)提供機體實時姿態(tài)信息和高度信息；③動力系統(tǒng)，由無刷電調(diào)、無刷電機及螺旋槳組成，電調(diào)接收PWM信號，再驅(qū)動電機帶動螺旋槳轉(zhuǎn)動。圖像處理系統(tǒng)包括從處理器、CMOS攝像頭處理模塊及FIFO組件等3部分。供電電池及電壓轉(zhuǎn)換模塊對整個系統(tǒng)提供3種不同的穩(wěn)定電壓，即11，5，3.3 V。無線遙控接收系統(tǒng)主要對飛行器進(jìn)行調(diào)試，使飛行器能平穩(wěn)飛行，然后記錄相關(guān)數(shù)據(jù)。

2 系統(tǒng)硬件設(shè)計

系統(tǒng)硬件主要包括飛行器平臺、動力系統(tǒng)、圖像系統(tǒng)及電源模塊四部分電路[5]。

2.1 飛行器平臺的電路設(shè)計

飛行器平臺的電路由微控制器最小系統(tǒng)、九軸姿態(tài)傳感器、超聲波傳感器和動力系統(tǒng)四部分組成。在此，主控制器采用STM32F407系列處理器。該芯片資源豐富，其最小系統(tǒng)包括時鐘電路、復(fù)位電路、SWD下載電路和I/O接口等。

九軸姿態(tài)傳感器MPU9150包括三軸陀螺儀、三軸加速計和三軸磁力計，可減少組合陀螺儀與加速計時的軸差現(xiàn)象[6]，同時加入磁力傳感器互補數(shù)據(jù)，以預(yù)防歐拉角奇異。此外，陀螺儀具有增強偏置和穩(wěn)定溫度的功能，姿態(tài)檢測模塊如圖2所示。

圖2 姿態(tài)檢測模塊Fig.2 Gesture detection module

高度傳感器運用US-100傳感器，此傳感器有2種工作模式，即利用I/O口傳輸數(shù)據(jù)和利用串口測試數(shù)據(jù)。由于UART模式操作簡便且不會浪費定時器，故釆用UART模式以達(dá)到測量高度的目的。

2.2 圖像處理系統(tǒng)的電路設(shè)計

圖像處理系統(tǒng)的電路設(shè)計由STM32F103最小系統(tǒng)電路和攝像頭處理電路組成，STM32F103相對STM32F407資源稍豐富。攝像頭數(shù)據(jù)采用SCCB總線協(xié)議傳輸，SCCB中文為串行攝像機控制總線，是一種與I2C總線相似且能夠兼容I2C的總線[4]，可以用GPIO直接模擬該總線時序，完成讀寫操作。

3 系統(tǒng)軟件設(shè)計

系統(tǒng)軟件設(shè)計的核心是主程序設(shè)計，具體包括系統(tǒng)初始化程序設(shè)計、姿態(tài)解算程序設(shè)計、定高程序設(shè)計和串級PID控制程序設(shè)計等。軟件開發(fā)工具釆用C語言，依靠結(jié)構(gòu)化與模塊化的核心設(shè)計思想，從后向前完成軟件設(shè)計。

3.1 系統(tǒng)主程序設(shè)計

控制系統(tǒng)的核心是主程序，其作用是結(jié)合所有驅(qū)動子程序及數(shù)據(jù)處理程序，完成計劃任務(wù)。首先，將所有模塊初始化，包含處理器內(nèi)部模塊如中斷設(shè)置、系統(tǒng)時鐘等。初始化后，進(jìn)入系統(tǒng)自檢，自檢通過才能執(zhí)行下面的程序。主函數(shù)功能分為兩部分，一部分是遙控調(diào)試，另一部分是一鍵啟動，這是本系統(tǒng)的主要執(zhí)行方式。當(dāng)運行任務(wù)時，各種傳感器會不斷采集所需要的信息，然后經(jīng)過數(shù)據(jù)融合，再計算控制量，最終得到控制量的輸出。另外運用多種中斷使程序更加穩(wěn)定高效。主程序流程如圖3所示。

圖3 系統(tǒng)總流程Fig.3 System total flow chart

3.2 系統(tǒng)初始化程序設(shè)計

系統(tǒng)初始化主要針對外設(shè)和控制系統(tǒng)中的其他模塊進(jìn)行初始化，使之能夠穩(wěn)定正常的工作。在系統(tǒng)中，還需要對MPU9150、攝像頭模塊、滴答定時器和中斷分組進(jìn)行初始化，因此整個飛控系統(tǒng)還需要設(shè)置中斷優(yōu)先級組別。攝像頭模塊的初始化相對復(fù)雜，需要設(shè)置數(shù)十個寄存器數(shù)值，主要設(shè)置圖像的格式、亮度、對比度、分辨率、圖像尺寸等。設(shè)計中采用QVAG格式，顏色模式為YUYV，圖像尺寸為320×240。

3.3 姿態(tài)解算程序設(shè)計

消除誤差的方法有互補濾波法、姿態(tài)插值法、卡爾曼濾波法等，其中互補濾波法最常見。描述姿態(tài)的方法有歐拉角、四元數(shù)、方向余旋矩陣等，其中四元數(shù)方法運算量最小且應(yīng)用廣泛，故選用四元數(shù)描述姿態(tài)。

設(shè)計姿態(tài)解算程序時，采用定時器中斷的方式以確保采樣頻率固定。采樣頻率為500 Hz，即每2 ms讀取1次傳感器數(shù)據(jù)，將得到的數(shù)據(jù)進(jìn)行滑動平均濾波處理，再與陀螺儀和加速度計的數(shù)據(jù)混合，最終采用四元數(shù)法解算出實時信息[7]。另外，偏航角的解算通過磁阻傳感器數(shù)據(jù)來調(diào)整，解決了飛機自我旋轉(zhuǎn)和漂移等現(xiàn)象，最后獲取到較好的姿態(tài)信息[8]。

3.4 定高程序設(shè)計

超聲波傳感器的數(shù)據(jù)讀取采用串口中斷的方法，串行口收到1個字節(jié)的數(shù)據(jù)就產(chǎn)生1次中斷。其具體流如下：先發(fā)出觸發(fā)指令0x55，等待串行口中斷，發(fā)生中斷時，需判斷收到數(shù)據(jù)的容量大小。如果為高字節(jié)，需要等待下次中斷來臨，反之則將上次中斷接收的高字節(jié)數(shù)據(jù)與本次低字節(jié)數(shù)據(jù)相融合，獲得所測量的高度。定高控制算法采用的是位置式PID控制，最后定高控制輸出與姿態(tài)控制輸出疊加到4個電機控制中。

在進(jìn)行基本的姿態(tài)控制后，飛行器能夠完成基本起飛任務(wù)，再進(jìn)行高度控制，即控制地理坐標(biāo)系中的Z軸。在此控制中，為使數(shù)據(jù)更加可靠穩(wěn)定，采用超聲波的融合數(shù)，檢測到飛行器的實際高度數(shù)據(jù)后，與期望高度進(jìn)行比較，得出偏差，然后用PID控制器修正偏差。也可采用串級PID控制器，外環(huán)為高度，內(nèi)環(huán)為上升速度。

3.5 串級PID控制的程序設(shè)計

針對飛行器的非線性，該控制系統(tǒng)選用角度外環(huán)PID和角速度內(nèi)環(huán)PID的串級PID控制器，其內(nèi)環(huán)控制回路是角度環(huán)與角速度環(huán)相結(jié)合。對干擾的反應(yīng)靈敏且具有抑制作用[9]及提高系統(tǒng)魯棒性，是角速度閉環(huán)控制的優(yōu)點，通常也稱增穩(wěn)環(huán)節(jié)[10]。四旋翼飛行器串級PID的流程如圖4所示。

圖4 串級PID流程Fig.4 Cascade PID flow chart

3.6 主控與協(xié)控的通信協(xié)議

圖像處理系統(tǒng)往飛行控制處理系統(tǒng)發(fā)送的數(shù)據(jù)中，有飛行器離目標(biāo)X軸偏差、飛行器離目標(biāo)Y軸偏差、飛行降落標(biāo)志位，然而圖像處理系統(tǒng)與飛行處理系之間通過串口通信，單一地發(fā)送數(shù)據(jù)使得飛行控制系不能正確地收到數(shù)據(jù)。因此，圖像處理系統(tǒng)與飛行控制處理系統(tǒng)之間需要通信協(xié)議，圖像處理系統(tǒng)采用這一協(xié)議發(fā)送數(shù)據(jù)，飛行控制系統(tǒng)按這一通信協(xié)議解析數(shù)據(jù)，使得圖像處理系統(tǒng)與飛行控制系統(tǒng)可以正常收發(fā)數(shù)據(jù)。通信采用RS232串口通信，通信的幀格式見表1。

表1 通信的幀協(xié)議Tab.1 Communication frame protocol

4 視覺尋跡算法的設(shè)計與實現(xiàn)

通過采集攝像頭傳感器對路徑的圖像信息，結(jié)合預(yù)處理和中心線提取，確定黑線位置，再通過視覺導(dǎo)航建立飛行器在水平面上的相對位置，最后通過位置控制得到需要補償?shù)淖藨B(tài)角，可方便調(diào)節(jié)飛行時對照線路的偏差，使飛行器在尋跡飛行時可準(zhǔn)確按照飛行線路飛行。

4.1 圖像采集原理

圖像時序如圖5所示。VSYN為2個正脈沖之間的時間，為一個完整圖像；HREF為掃描該圖像中各行像素的時間，高電平時為有效時間；PCLK表示同步信號由有效像素值提供，高電平時輸出有效圖像數(shù)據(jù)。若當(dāng)前圖像窗口大小為320×240，則在2個正脈沖之間有240個HREF的正脈沖，即240行；在每個HREF正脈沖期間有320個PCLK正脈沖，即每行320個像素[5]。

圖5 圖像的時序圖Fig.5 Timing diagram of image

4.2 圖像的預(yù)處理

通常，機載攝像頭獲取圖像會受到無人機的振動、環(huán)境因素的影響，存在噪聲干擾、失真等問題，使圖像質(zhì)量明顯下降。為了提高圖像的精度，在提取目標(biāo)圖像的特征前，必須采取一定措施用于降低干擾的影響，以方便后續(xù)處理如圖像灰度與二值化和濾波。

4.3 賽道提取算法

路徑識別系統(tǒng)中重要的環(huán)節(jié)是提取黑線中心部分，根據(jù)系統(tǒng)特性，采用中心線提取法來提取路徑。具體過程是：先判斷每行的起始點是否為黑點，若不是則對白點計數(shù)（設(shè)計數(shù)為x），當(dāng)為連續(xù)黑點時計黑點個數(shù)（設(shè)計數(shù)為y），再次為白點時退出計數(shù)，這時黑線中心位于第（x+y/2）列；若起始點為黑點，且不連續(xù)，則對黑點計數(shù)（設(shè)計數(shù)為b），當(dāng)遇到連續(xù)白點后退出計數(shù)，此時黑線中心位于第（b/2）列。最后，將得到的所有中心位置，按順序儲存在數(shù)組中[5]。雖然已去除噪音，但仍有干擾會使某些黑線中心位置產(chǎn)生誤差，為此需要在產(chǎn)生誤差的地方做差值處理，則黑線中心值即為前后2行的平均值[7]。

4.4 視覺導(dǎo)航

為準(zhǔn)確描述四旋翼飛行器的空間運動狀態(tài)信息，需定義以下坐標(biāo)系[11]：

攝像頭坐標(biāo)系Γc在機身下方安裝攝像頭，設(shè)光心與飛機質(zhì)心重合，坐標(biāo)系的原點Oc為光心，Zc軸為光軸，Xc軸和Yc軸為2條垂邊，分別與圖像平面平行[11]。

圖像坐標(biāo)系xOy攝像頭成像原理與小孔成像原理相似，會產(chǎn)生跑道軌跡與所投影的圖像分別在攝像頭光心的兩側(cè)且圖像倒立等現(xiàn)象，故設(shè)成像平面在攝像頭的光心和跑道之間[11]。

像素坐標(biāo)系uO′v坐標(biāo)單位為像素，坐標(biāo)系原點O′位于圖像的左上角，u軸和v軸分別平行于圖像坐標(biāo)的x軸和y軸[11]。

由于QUAV屬于低空低速飛行狀態(tài)，攝像頭的焦距遠(yuǎn)小于跑道軌跡到攝像頭光心的距離，因此攝像頭成像可認(rèn)為是小孔成像原理[11]，其模型如圖6所示。

圖6 機體坐標(biāo)與攝像頭坐標(biāo)系的轉(zhuǎn)換關(guān)系和透視投影模型Fig.6 Transforming relation and perspective projection model between body coordinate and camera coordinate system

飛行器通過攝像頭識別跑道軌跡信息時，提取圖像特征后，當(dāng)跑道路線中心線上中心點的投影點P，與機體質(zhì)心投影點O位于同一點時，說明飛行器正好在跑道路線上方飛行；未重合則說明飛行器與跑道路線有偏差，需調(diào)節(jié)位置偏差[11]，由此奠定四旋翼循跡自主飛行基礎(chǔ)并提供位移控制量[5-11]。

4.5 位置控制

設(shè)軌跡中心線上的中心點在地面坐標(biāo)系下的坐標(biāo)為［xd，yd，0］T， 圖像坐標(biāo)系的原點為機體質(zhì)心在圖像上的投影點，設(shè)其坐標(biāo)為y0-y1分別為飛行器相對于軌跡的左右位移差、距離位移差；x0-xi，y0-yi分別為飛行器左右、前后平移速度誤差。設(shè)z0為目標(biāo)飛行高度，飛機飛行過程中超聲波測量高度為zi，則高度誤差為z0-z1[11-12]。

5 系統(tǒng)測試與分析

定高控制PID的參數(shù)調(diào)試，可以先根據(jù)控制輸出的PWM值和高度數(shù)據(jù)的計算，把4個軸綁住大致確定出參數(shù)，再根據(jù)實際飛行效果最后確定出參數(shù)。定高控制的積分需要限幅，否則會導(dǎo)致積分誤差越來越大，使飛行器越飛越高，積分限幅的大小由四軸飛行器本身的重力決定。四軸飛行器定高控制高度數(shù)據(jù)波形如圖7所示。

圖7 四軸飛行器定高調(diào)試Fig.7 Fixed altitude adjustment of four axis rotorcraft

利用keil5實現(xiàn)在線調(diào)試，如圖8所示，RC_Pwm_In 的［0］—［3］分別為 ROLL，PITCH，THR（油門）及 YAW 的實際值，Distance［2］為超聲波的高度值，291為超聲波的測量高度 29.1 cm；Roll，Pitch及Yaw為陀螺儀解算出來的當(dāng)前姿態(tài)角。黑線正中間是160，即飛行器處于正中間。OV7670的分辨率為320×240，所以0代表最左端，319代表最右端。

四旋翼飛行器自主飛行的試驗效果如圖9所示，懸停效果如圖10所示。

圖8 飛控平臺的整體調(diào)試Fig.8 Overall debugging of the flight control platform

圖9 實際飛行調(diào)試Fig.9 Actual flight debugging

圖10 懸停效果測試Fig.10 Hover effects test

6 結(jié)語

對四旋翼飛行器進(jìn)行了軟硬件設(shè)計，在飛控平臺上開展了自主尋跡飛行的研究與設(shè)計。通過超聲波傳感器進(jìn)行高度采集，經(jīng)過高度PID進(jìn)行高度的定位；通過姿態(tài)測量傳感器進(jìn)行姿態(tài)分析，再通過姿態(tài)解算進(jìn)行平衡調(diào)整；通過中心線提取法對黑線路徑進(jìn)行了提取，通過視覺導(dǎo)航得出了飛行的實際位置，再通過循跡算法得出了相應(yīng)控制量再融入姿態(tài)數(shù)據(jù)進(jìn)行結(jié)算，便于與位置控制器解得到的結(jié)果作差，提高了調(diào)節(jié)實際飛行軌跡與跑道軌跡偏差的靈敏性，使四旋翼飛行器在自主尋跡飛行時能準(zhǔn)確按照跑道軌跡飛行，如果飛行過程中出現(xiàn)位置偏差，可及時調(diào)整好飛行方向。

[1]楊明志，王敏.四旋翼微型飛行器控制系統(tǒng)設(shè)計[J].計算機測量與控制，2008，16（4）：485-490.

[2]凌金福.四旋翼飛行器飛行控制算法的研究[D].南昌：南昌大學(xué)，2013.

[3]Salih A L，Moghavvemi M，Mohamed H A F，et al.Modelling and PID controller design for a quadrotor unmanned air vehicle[C]//Automation Quality and Testing Robotics （AQT R）.2010 IEEE International Conference on IEEE，2010（1）：1-5.

[4]劉煥曄.小型四旋翼飛行器飛行控制系統(tǒng)研究與設(shè)計[D].上海：上海交通大學(xué)，2009.

[5]Xu L F，Jiang S D.Research of intelligence control for flying altitude of four rotors flyer[C]//International Conference on ArtificialIntelligence and ComputationalIntelligence.2010：294-298.

[6]陳海濱，殳國華.四旋翼飛行器的設(shè)計[J].實驗室研究與探索，2013，32（3）：41-44.

[7]王福，鮮斌，黃國平，等.四旋翼無人機自主懸?？刂蒲芯縖C]//2013年第32屆中國控制會議論文集.西安：中國自動化學(xué)會控制理論專委會、中國系統(tǒng)工程學(xué)會，2013：620-625.

[8]于雅莉，孫楓，王元昔.基于多傳感器的四旋翼飛行器硬件電路設(shè)計[J].傳感器與微系統(tǒng)，2011，30（8）：113-115.

[9]李俊，李運堂.四旋翼飛行器的動力學(xué)建模及PID控制[J].遼寧工程技術(shù)大學(xué)學(xué)報，2012，31（1）：114-117.

[10]岳基隆，張慶杰，朱華勇.微小型四旋翼無人機研究進(jìn)展及關(guān)鍵技術(shù)淺析[J].電光與控制，2010，17，（10）：46-52.

[11]張婧.四旋翼無人機自主飛行控制系統(tǒng)設(shè)計與開發(fā)[D].?？冢汉Ｄ洗髮W(xué)，2015.

[12]劉峰，呂強，王國勝，等.四軸飛行器姿態(tài)控制系統(tǒng)設(shè)計[J].計算機測量與控制，2011，19（3）：583-585.