基于RX23T的四旋翼飛行器控制系統(tǒng)設(shè)計(jì)＊

曹繼華，梁 偉，劉 杰，姚慧雄，向曉燕

(吉首大學(xué)信息科學(xué)與工程學(xué)院，湖南 吉首 416000)

0 引言

近年來(lái)，隨著微電子技術(shù)的發(fā)展和控制理論的進(jìn)一步發(fā)展，四旋翼飛行器已發(fā)展成為集軍用，商用，民用于一體的多功能無(wú)人駕駛工具，并不斷有新的功能加載到它的上面[1]。

本設(shè)計(jì)使用了R5F523T5ADFM單片機(jī)作為四旋翼飛行器的控制核心，在對(duì)獲取的圖像數(shù)據(jù)進(jìn)行分析后，結(jié)合PID控制算法，通過(guò)編程，仿真和反復(fù)的調(diào)試，最終實(shí)現(xiàn)了四旋翼飛行器對(duì)地面運(yùn)動(dòng)目標(biāo)跟蹤飛行的功能。

1 四旋翼飛行原理

四旋翼飛行器的四個(gè)旋翼都位于同一水平面，飛行器通過(guò)控制旋翼上四個(gè)電機(jī)的轉(zhuǎn)速來(lái)改變旋翼升力的大小，從而實(shí)現(xiàn)四旋翼飛行器姿態(tài)穩(wěn)定。相鄰位置螺旋槳的旋轉(zhuǎn)方向是相反的，正好抵消了四旋翼飛行器的正反力矩，因此可以保證四個(gè)電機(jī)同時(shí)轉(zhuǎn)動(dòng)而機(jī)身不轉(zhuǎn)動(dòng)。當(dāng)某兩個(gè)相鄰的電機(jī)轉(zhuǎn)速減小而另一對(duì)相鄰電機(jī)轉(zhuǎn)速增大，則會(huì)產(chǎn)生向某個(gè)方向運(yùn)動(dòng)的速度[2]。本設(shè)計(jì)通過(guò)測(cè)量器件感知飛行器姿態(tài)發(fā)生變化，將測(cè)量到的姿態(tài)信息發(fā)送到處理器處理，經(jīng)過(guò)控制器解算后輸出到執(zhí)行機(jī)構(gòu)，控制了電機(jī)的轉(zhuǎn)速，使得飛行器按照期望的姿態(tài)穩(wěn)定飛行。由于采用的是負(fù)反饋閉環(huán)控制，所以整個(gè)四旋翼飛行器系統(tǒng)會(huì)逐漸趨向于穩(wěn)定的狀態(tài)。

2 系統(tǒng)的硬件平臺(tái)設(shè)計(jì)

硬件平臺(tái)設(shè)計(jì)主要采用兩塊R5F523T5ADFM單片機(jī)和若干傳感器，其中一塊R5F523T5ADFM芯片與姿態(tài)傳感器模塊構(gòu)成飛行控制板，主要對(duì)飛行器的飛行姿態(tài)進(jìn)行控制。另一塊R5F523T5ADFM芯片與測(cè)距傳感器模塊，圖像傳感器模塊構(gòu)成導(dǎo)航數(shù)據(jù)處理板，將傳感器原始數(shù)據(jù)經(jīng)處理后上傳到飛行控制板，從而對(duì)飛行器實(shí)現(xiàn)導(dǎo)航。系統(tǒng)硬件組成如圖1所示。

圖1 系統(tǒng)組成框圖

2.1 主控芯片的選擇

本系統(tǒng)的硬件平臺(tái)采用R5F523T5ADFM單片機(jī)作為控制核心，其具有一個(gè)32bit的CPU核心,4Gbyte尋址空間，內(nèi)含16個(gè)通用寄存器，10個(gè)控制寄存器，2個(gè)72位累加寄存器，最高系統(tǒng)時(shí)鐘可達(dá)40MHz。同時(shí)該芯片集成了128KbyteROM，12KbyteRAM，以及各種片上外設(shè)。具有豐富的接口以滿(mǎn)足與各種傳感器的連接通信，和較強(qiáng)的性能使得從傳感器采集的數(shù)據(jù)得到實(shí)時(shí)處理。

2.2 姿態(tài)傳感器模塊的選擇

采用MPU9250芯片內(nèi)置的三軸加速度計(jì)，三軸陀螺儀以及三軸磁力計(jì)獲取原始姿態(tài)數(shù)據(jù)，經(jīng)解算后得到飛行器的姿態(tài)數(shù)據(jù)。其中陀螺儀可編程其量程為（±250，±500，±1000度/秒），可進(jìn)行三軸（x，y，z）16位ADC角速度數(shù)字輸出。加速度計(jì)可編程其量程為（±2g，±4g，±8g，±16g），可進(jìn)行三軸16位ADC加速度數(shù)字輸出。

2.3 測(cè)距傳感器模塊的選擇

采用KS103超聲波模塊對(duì)飛行器飛行高度進(jìn)行測(cè)量。該超聲波模塊包含實(shí)時(shí)溫度補(bǔ)償?shù)木嚯x探測(cè)，具有高探測(cè)精度。探測(cè) 頻率可達(dá)500Hz，使用 I2C/串口串口接口與主機(jī)通信，自動(dòng)響應(yīng)主機(jī)的I2C/串口串行口控制指令，短距探測(cè)量程由10cm、20cm、……、至470cm，能滿(mǎn)足快速近距離探測(cè)，從而實(shí)現(xiàn)室內(nèi)實(shí)時(shí)處理高度數(shù)據(jù)。

2.4 圖像傳感器模塊的選擇

采用曝光芯片為OV7725，包含一塊AL422B板上FIFO芯片的攝像頭模塊。OV7725感光芯片在低光照情況下也具有高靈敏度，標(biāo)準(zhǔn)的SCCB通信接口，支持RGB彩色模式數(shù)據(jù)輸出，同時(shí)具有幀同步能力，采集的彩色圖像數(shù)據(jù)通過(guò)8位并口傳輸給單片機(jī)，具有滿(mǎn)足實(shí)時(shí)數(shù)據(jù)傳輸?shù)乃俣龋冶ＷC圖像信息采集的完整度。用來(lái)為飛行器采集地面圖像信息，從而使飛行器得以在室內(nèi)實(shí)現(xiàn)循跡飛行，跟蹤飛行和定點(diǎn)懸停。

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

3.1 循跡模塊算法設(shè)計(jì)

為了保證飛行器能夠?qū)崟r(shí)有效的獲取目標(biāo)的位置信息，考慮采用機(jī)器視覺(jué)的方法對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行定位[3]。采用一塊R5F523T5ADFM單片機(jī)進(jìn)行導(dǎo)航控制。導(dǎo)航板通過(guò)視覺(jué)傳感器獲取地面的圖像數(shù)據(jù)，經(jīng)過(guò)一定的控制算法處理后得到小車(chē)和圓心位置信息[5]，并通過(guò)串口發(fā)送給飛控主控芯片從而達(dá)到循跡的效果。

3.1.1 圖像獲取

為了能夠更準(zhǔn)確的獲取飛行器當(dāng)前狀態(tài)下的所掃描到的賽道信息，將攝像頭傳感器安裝在飛行器下方的中間位置。配置ov7725攝像頭的寄存器，將其輸出窗口設(shè)置成60*80，并且能夠自動(dòng)調(diào)節(jié)邊緣的增強(qiáng)、自動(dòng)調(diào)節(jié)噪聲的抑制。同時(shí)單片機(jī)配置相應(yīng)的輸入輸出端口和中斷觸發(fā)源，根據(jù)攝像頭傳感器內(nèi)部產(chǎn)生的場(chǎng)同步信號(hào)和行同步信號(hào)進(jìn)行圖像采集并保存到緩存區(qū)。

賽道是由黑色和白色兩種背景顏色組成，故采集時(shí)可將圖像二值化為0和1兩種數(shù)值(0為黑色，1為白色)，以便于數(shù)據(jù)的保存和處理。數(shù)學(xué)表達(dá)式如公式⑴。

閥值(Threshold)用來(lái)把目標(biāo)和背景區(qū)分開(kāi)。由于賽道背景顏色單一，故可以通過(guò)采集幾幀不同狀態(tài)下的圖像用來(lái)提取靜態(tài)閾值。

3.1.2 目標(biāo)識(shí)別算法

從采集的圖像中計(jì)算出目標(biāo)物體的位置信息是飛行器自主飛行過(guò)程中核心的部分，目標(biāo)檢測(cè)識(shí)別算法設(shè)計(jì)思路如圖2所示，主要分為以下三步。

⑴ 構(gòu)造一個(gè)虛擬的初始為3*3的方形矩陣模板，矩陣的大小可以根據(jù)掃描的層數(shù)N(初始值為1)擴(kuò)展，定義一個(gè)數(shù)組分別記錄矩陣四個(gè)角位置相對(duì)于矩陣中心的偏移量。

⑵ 對(duì)輸入的二值圖像進(jìn)行遍歷，當(dāng)掃描到黑點(diǎn)（數(shù)值信息為0）時(shí)，則以該像素點(diǎn)的位置為基點(diǎn)，匹配步驟⑴中的矩陣模板，判斷四個(gè)角位置的像素點(diǎn)是否為黑點(diǎn)。若都為黑點(diǎn)，則增加層數(shù)(N+1)，擴(kuò)展虛擬矩陣并更新數(shù)組的值繼續(xù)判斷，直到遇到白點(diǎn)(數(shù)值信息為1)結(jié)束，同時(shí)記錄當(dāng)前掃描的層數(shù)和基點(diǎn)的位置。

⑶ 對(duì)每次得到的層數(shù)的大小進(jìn)行比較，取最大層數(shù)所對(duì)應(yīng)的基點(diǎn)位置則是圓心。

圖2 目標(biāo)檢測(cè)算法流程圖

3.2 姿態(tài)解算模塊算法設(shè)計(jì)

姿態(tài)解算是飛控系統(tǒng)的基礎(chǔ)，系統(tǒng)采用的姿態(tài)結(jié)算，采用了四元數(shù)法，設(shè)計(jì)思路如圖3所示。姿態(tài)解算得出的姿態(tài)數(shù)據(jù)會(huì)發(fā)布給姿態(tài)控制器，控制四旋翼飛行平穩(wěn)。姿態(tài)解算的速度和精度，會(huì)直接影響系統(tǒng)的實(shí)時(shí)性和穩(wěn)定性。姿態(tài)是用來(lái)描述一個(gè)剛體的固連坐標(biāo)系和參考坐標(biāo)系之間的角位置關(guān)系，常用的姿態(tài)表示方法有：歐拉角、方向余弦矩陣、四元數(shù)表示[4]。相比其他的表示方法，四元數(shù)法計(jì)算量小，算法簡(jiǎn)單，易于操作，避免了歐拉角的奇異性問(wèn)題。使用四元數(shù)來(lái)更新姿態(tài)，最后轉(zhuǎn)化為歐拉角發(fā)布給姿態(tài)控制器。

圖3 姿態(tài)解算模塊算法

四元數(shù)q=q0+q1i+q2j+q3k，歐拉角、方向余弦矩陣、四元數(shù)的關(guān)系如公式⑵。

陀螺儀測(cè)量角速度，具有高動(dòng)態(tài)性，受溫度和電壓影響產(chǎn)生零點(diǎn)漂移現(xiàn)象，離散采樣產(chǎn)生累積誤差。加速度計(jì)的低頻特性好，可以測(cè)量低速狀態(tài)下的加速度。結(jié)合兩者的優(yōu)點(diǎn)，姿態(tài)解算的算法為：根據(jù)加速度計(jì)數(shù)據(jù)，轉(zhuǎn)換到地理坐標(biāo)系后，與對(duì)應(yīng)參考的重力向量進(jìn)行求誤差，這個(gè)誤差用來(lái)校正陀螺儀的輸出，然后用陀螺儀數(shù)據(jù)進(jìn)行四元數(shù)更新，再轉(zhuǎn)換到歐拉角。

四元數(shù)姿態(tài)更新采用一階畢卡算法進(jìn)行，如（公式3）所示。其中Δt代表采樣時(shí)間間隔，ωxb、ωyb、ωzb為載體坐標(biāo)系下的角速度。

3.3 控制器設(shè)計(jì)

本系統(tǒng)采用串級(jí)PID控制，包括位置控制環(huán)和姿態(tài)控制環(huán)，姿態(tài)控制環(huán)內(nèi)部又采用了雙環(huán)PID，內(nèi)回路為姿態(tài)角速度PID控制，外環(huán)為姿態(tài)角度P控制。遙控器或者上位機(jī)設(shè)定期望位置后，位置PID回路計(jì)算出所需的姿態(tài)角，送給姿態(tài)控制環(huán)，姿態(tài)控制環(huán)計(jì)算電機(jī)控制量，并轉(zhuǎn)化為相應(yīng)的PWM波輸入給電機(jī)。系統(tǒng)控制器結(jié)構(gòu)如圖4所示。

圖4 系統(tǒng)控制器結(jié)構(gòu)圖

4 飛行試驗(yàn)結(jié)果

⑴ 起飛捕獲能力測(cè)試

測(cè)試方式：飛行器平穩(wěn)放置水平面，小車(chē)靜止于飛行器正前方一米處，飛行器一鍵起飛向前飛行，當(dāng)捕獲到小車(chē)位置時(shí)，懸停于小車(chē)正上方。飛行器起飛速度與小車(chē)位置捕獲之間的關(guān)系如表1所示。

表1 起飛捕獲能力測(cè)試

分析：飛行器需要以一個(gè)合適的速度飛向小車(chē)，若速度太快不容易懸停于小車(chē)上，若太慢則容易偏離小車(chē)所在方向。

⑵ 跟蹤飛行能力測(cè)試

測(cè)試方式：在起飛捕獲到小車(chē)位置并懸停于小車(chē)上方之后，小車(chē)開(kāi)始運(yùn)動(dòng)，觀察飛行器跟蹤小車(chē)的飛行情況。飛行器跟蹤能力與小車(chē)運(yùn)動(dòng)速度之間的關(guān)系如表2所示。

表2 跟蹤飛行測(cè)試

分析：跟蹤飛行時(shí)，小車(chē)的運(yùn)動(dòng)速度不宜太快，否則飛行器難以及時(shí)調(diào)整飛行方向，會(huì)失去對(duì)小車(chē)的跟蹤。最終測(cè)試效果如圖5所示。測(cè)試結(jié)果表明，所設(shè)計(jì)的飛行器及控制系統(tǒng)對(duì)地面運(yùn)動(dòng)目標(biāo)可實(shí)現(xiàn)穩(wěn)定、實(shí)時(shí)及精確的跟蹤飛行。

圖5 系統(tǒng)測(cè)試效果圖

5 結(jié)束語(yǔ)

該四旋翼飛行器探測(cè)跟蹤系統(tǒng)是基于兩塊R5F523T5ADFM單片機(jī)，一塊進(jìn)行姿態(tài)解算及飛行控制，另一塊進(jìn)行圖像數(shù)據(jù)處理并計(jì)算出運(yùn)動(dòng)目標(biāo)位移信息，通過(guò)UART接口實(shí)現(xiàn)雙機(jī)通信。再結(jié)合相應(yīng)的控制算法進(jìn)行解算，輸出到執(zhí)行機(jī)構(gòu)，改變電機(jī)轉(zhuǎn)速，完成四旋翼飛行器跟蹤地面運(yùn)動(dòng)小車(chē)的運(yùn)動(dòng)控制。無(wú)人機(jī)自動(dòng)跟蹤系統(tǒng)結(jié)合了傳感器、控制理論、空氣動(dòng)力學(xué)、電機(jī)學(xué)、圖像處理等多門(mén)學(xué)科的最新理論和實(shí)踐技術(shù)，其應(yīng)用前景相當(dāng)廣闊。將無(wú)人機(jī)運(yùn)用到地面移動(dòng)目標(biāo)的跟蹤中，能有效克服傳統(tǒng)視覺(jué)跟蹤監(jiān)控范圍限制、目標(biāo)易丟失等問(wèn)題，是視覺(jué)跟蹤理論當(dāng)前的研究熱點(diǎn)。為提高系統(tǒng)的實(shí)用性，未來(lái)將在擾動(dòng)作用下對(duì)飛行器穩(wěn)定性及跟蹤速度、精度等方面進(jìn)一步研究。

參考文獻(xiàn)(References):

[1]魏麗娜,寧會(huì)峰,陸旺等.多旋翼飛行器的關(guān)鍵技術(shù)及應(yīng)用前景[J].自動(dòng)化技術(shù)與應(yīng)用,2016.9:1-4

[2]古春海.基于APM平臺(tái)的無(wú)人機(jī)目標(biāo)追蹤系統(tǒng)研究[D].上海師范大學(xué)碩士學(xué)位論文,2016.

[3]樊瓏.多旋翼無(wú)人機(jī)視覺(jué)引導(dǎo)降落研究[D].哈爾濱工業(yè)大學(xué)碩士學(xué)位論文,2016.

[4]雷家勇,達(dá)飛鵬,孟廣猛.圖像處理中圓心算法研究[J].計(jì)算機(jī)與現(xiàn)代化,2005.3.

[5]溫全,成天樂(lè),蘇澤亞.基于STM32和OV2640的自主循跡四旋翼飛行器設(shè)計(jì)[J].微型機(jī)與應(yīng)用,2016.35(22):105-107