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

時(shí)夢(mèng)瑤，李建奇，黃瑞敏，張 斌，田中杰

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

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

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

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

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

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

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

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

2.1 飛行器平臺(tái)的電路設(shè)計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.4 定高程序設(shè)計(jì)

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

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

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

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

圖4 串級(jí)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)與飛行處理系之間通過(guò)串口通信，單一地發(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串口通信，通信的幀格式見(jiàn)表1。

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

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

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

4.1 圖像采集原理

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

圖5 圖像的時(shí)序圖Fig.5 Timing diagram of image

4.2 圖像的預(yù)處理

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

4.3 賽道提取算法

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

4.4 視覺(jué)導(dǎo)航

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

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

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

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

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

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

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

4.5 位置控制

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

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

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

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

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

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

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

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

圖10 懸停效果測(cè)試Fig.10 Hover effects test

6 結(jié)語(yǔ)

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

[1]楊明志，王敏.四旋翼微型飛行器控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，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è)計(jì)[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]陳海濱，殳國(guó)華.四旋翼飛行器的設(shè)計(jì)[J].實(shí)驗(yàn)室研究與探索，2013，32（3）：41-44.

[7]王福，鮮斌，黃國(guó)平，等.四旋翼無(wú)人機(jī)自主懸停控制研究[C]//2013年第32屆中國(guó)控制會(huì)議論文集.西安：中國(guó)自動(dòng)化學(xué)會(huì)控制理論專(zhuān)委會(huì)、中國(guó)系統(tǒng)工程學(xué)會(huì)，2013：620-625.

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

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

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

[11]張婧.四旋翼無(wú)人機(jī)自主飛行控制系統(tǒng)設(shè)計(jì)與開(kāi)發(fā)[D].海口：海南大學(xué)，2015.

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