巫子晨，楊文韜，孫蓬勃

（大連理工大學(xué)軟件學(xué)院，大連116621）

1 需求分析

四旋翼飛行器是一種四螺旋槳驅(qū)動的、可垂直起降的飛行器[1]，其已廣泛應(yīng)用于軍事、民用等諸多領(lǐng)域[2]。對于傳統(tǒng)的四旋翼飛行器，只能通過專用遙控器進(jìn)行控制，功能單一、自動化低，飛行過程中需要用于保持操作狀態(tài)，需要較多技巧，對于非專業(yè)人群來說十分不便，容易出現(xiàn)意外，而且無法實時了解飛行器飛行時的狀況，導(dǎo)致無法及時發(fā)現(xiàn)問題并做出調(diào)整。

基于以上分析，在控制端應(yīng)用方面，需要一個簡潔友好的控制界面方便用戶控制，例如運用手機(jī)App直接對飛行器進(jìn)行控制，以替代傳統(tǒng)的遙控器?；蚴侵苯油ㄟ^PC端應(yīng)用程序控制，在控制端實時獲取飛行器的狀態(tài)參數(shù)，發(fā)布飛行器任務(wù)，例如路線規(guī)劃，自動投遞等。在飛行器設(shè)計方面，要具有一定的抗干擾能力和較高的自動化程度，以方便用戶使用，例如自動起飛和降落、自動定高和定點，當(dāng)受到外界干擾時，能通過自我調(diào)節(jié)保持穩(wěn)定等。

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

本系統(tǒng)的主要工作邏輯如下：四旋翼飛行器啟動，按照設(shè)計程序執(zhí)行飛行任務(wù)；Android App通過Wi-Fi連接四旋翼飛行器，監(jiān)測并控制飛行器當(dāng)前狀態(tài)；上位機(jī)通過藍(lán)牙連接至飛行器，輔助控制飛行器。系統(tǒng)結(jié)構(gòu)層次見圖1：本系統(tǒng)的主要功能模塊劃分為：目標(biāo)機(jī)模塊、Android端模塊和上位機(jī)模塊。其中以四旋翼飛行器為核心，Android端與上位機(jī)則負(fù)責(zé)監(jiān)測飛行器狀態(tài)并施以輔助控制。

2.1 目標(biāo)機(jī)模塊設(shè)計

該模塊為四旋翼飛行器硬件平臺上的軟件部分，層次關(guān)系如圖2：

四旋翼飛行器的硬件部分以STM32F4系列開發(fā)板為主控板，將其與機(jī)架、電機(jī)等飛行器硬件組裝后得到硬件平臺。軟件部分包括飛行器底層驅(qū)動，例如氣壓計、磁力計、加速度計等傳感器的驅(qū)動，以及飛行器姿態(tài)解算和飛控算法軟件。

圖2 四軸軟件與硬件兩方面內(nèi)容

四旋翼飛行器實現(xiàn)定高、定點、循跡、避障等基礎(chǔ)功能，并與終端實時通信，不斷反饋機(jī)體自身以及周圍環(huán)境信息。其中關(guān)鍵性問題分為兩個：傳感器的數(shù)據(jù)融合算法、飛行控制算法。

（1）傳感器的數(shù)據(jù)融合算法

陀螺儀動態(tài)響應(yīng)特性良好，但計算姿態(tài)時會產(chǎn)生累積誤差。磁力計和加速度計測量姿態(tài)沒有累積誤差，但動態(tài)響應(yīng)較差。因此他們在頻域上特性互補，可以采用互補濾波器融合這三種傳感器的數(shù)據(jù)，提高測量精度和系統(tǒng)的動態(tài)性能。

設(shè)計的互補濾波器的傳遞函數(shù)為：

其中C(S)表示飛行器運動過程中真實的姿態(tài)矩陣;C?(S)表示互補濾波器估計得姿態(tài)矩陣;Co表示加速度計和磁力計觀測到的姿態(tài)矩陣(Co=C+μH，μH為觀測到的高頻噪聲);Gω為陀螺儀測量得到的姿態(tài)矩陣（Cω=C+μL，μL為低頻誤差累積）;具有一階低通濾波特性，G2具有一階高通濾波特性，因此通過互補濾波器能夠消除高頻噪聲和低頻誤差的累積，能夠很好地融合各傳感器數(shù)據(jù)。

互補濾波姿態(tài)解算的步驟如圖3所示。

圖3 互補濾波姿態(tài)解算步驟圖

在姿態(tài)計算初始時，通過陀螺儀采集數(shù)據(jù)獲得的飛行器初始姿態(tài)角計算得到初始時刻的四元數(shù)，即q0、q1、q2、q3；讀取加速度計的值以及磁強(qiáng)計的值并將其測量值轉(zhuǎn)化為單位向量通過初始時刻的四元數(shù)計算獲得磁場向量w?b與重力向量?b，并分別與?b和?b進(jìn)行叉乘得到計算姿態(tài)與實際姿態(tài)的誤差eφ與eθ，利用處理后的誤差值δ修正陀螺儀的采樣值ωk得到修正的陀螺儀數(shù)值ω，將ω作為系數(shù)Ω更新四元數(shù)初始值q并作規(guī)范化得到最終值q?，將四元數(shù)轉(zhuǎn)換成歐拉角輸出姿態(tài)矩陣C?。

（2）飛行控制算法

四旋翼飛行器的控制目標(biāo)是使其在無動作指令時保持穩(wěn)定懸停狀態(tài)，有動作指令時有效完成指定動作。串級PID控制器結(jié)構(gòu)簡單、穩(wěn)定性好、工作可靠、調(diào)整方便，在工業(yè)控制中被廣泛使用。使用串級PID控制器進(jìn)行飛行器的姿態(tài)控制，原理如圖4所示：

圖4 串級PID原理圖

在四旋翼飛行器中，將飛行器的姿態(tài)角度作為外環(huán)的PID輸入值，用角速度作為內(nèi)環(huán)PID的輸入值，串級PID的輸出值經(jīng)過處理后直接累加至PWM值輸出至電機(jī)，其示意圖如圖5所示：

圖5 飛行姿態(tài)PID控制器原理圖

以姿態(tài)歐拉角的期望值與計算值之差作為PID控制器的輸入，每個電機(jī)對應(yīng)的PWM控制量都是三個PID控制器輸出的疊加，疊加量的正負(fù)與電機(jī)位置相關(guān)。姿態(tài)控制與升力控制相結(jié)合，使飛行器能夠完成三維空間的各種運動。飛行器懸停可拆分為定高與定點功能，定高使用超聲波模塊輸出值作為PID高度環(huán)輸入值，將氣壓計采集數(shù)據(jù)的微分作為速度環(huán)的輸入值，通過串級PID調(diào)控達(dá)到穩(wěn)定在某一高度飛行的效果。定點功能的實現(xiàn)與定高功能類似。在室外采用GPS實現(xiàn)定點，通過GPS采集的經(jīng)緯度坐標(biāo)與目的坐標(biāo)經(jīng)緯度進(jìn)行比較并作處理后得到位置的偏差量。將該偏差量放大后作為單級PID的輸入，并將PID輸出作為飛行器的目標(biāo)姿態(tài)，從而實現(xiàn)GPS定點。由于室內(nèi)無法收到GPS信號，故飛行器在室內(nèi)采用光流定點，使用的控制算法為串級PID。通過光流模塊采集地面圖像移動速度，濾波并積分，然后再以加速度計采集的數(shù)據(jù)校正，從而得到飛行器在平面內(nèi)的相對位置，達(dá)到定點效果，定高與定點結(jié)合從而實現(xiàn)懸停效果。

2.2 Android端模塊

Android端功能主要分為兩個子模塊，即界面設(shè)計與通信協(xié)議設(shè)計：

（1）界面設(shè)計

本系統(tǒng)的Android App界面布局如圖6：

圖6 Android App界面布局設(shè)計

安卓界面設(shè)計采用透明化風(fēng)格，沒有視頻時默認(rèn)為背景色，圖標(biāo)控件均為偏深透明色，滿足操作的同時可以看到視頻流的要求。同時，偏移數(shù)據(jù)實時顯示在界面上，使用戶有數(shù)據(jù)化操作體驗。

（2）通信協(xié)議設(shè)計

由于在安卓端與飛行器通信的過程中需要發(fā)送多種格式的消息，為了避免由于丟包或其他原因而造成通信紊亂，設(shè)計了飛行器與終端接發(fā)消息的通信協(xié)議，如圖7所示：

圖7 部分通信協(xié)議

其中在Android端讀取飛行器發(fā)送的消息時，安卓端先發(fā)出讀請求，請求的前兩個字節(jié)0xA55A為命令頭，第三個字節(jié)代表當(dāng)前消息的總長度，第四個字節(jié)表示希望讀取數(shù)據(jù)的ID。在安卓端向飛行器發(fā)送寫命令時，消息前兩個字節(jié)為命令頭，第三個字節(jié)0xFF代表當(dāng)前命令為寫請求，從第四個字節(jié)開始為安卓端向飛行器寫入的具體數(shù)據(jù)，當(dāng)所有數(shù)據(jù)發(fā)送完畢時，安卓端接著發(fā)送0xA5AA兩個字節(jié)表示發(fā)送完畢。

2.3 上位機(jī)模塊

上位機(jī)模塊即PC端地面監(jiān)測站，用于輔助安卓端進(jìn)行控制，使用美國國家儀器公司開發(fā)的LabVIEW軟件進(jìn)行開發(fā)。在上位機(jī)與飛行器進(jìn)行藍(lán)牙通信時，可使用LabVIEW提供的VISA組件，對于串口通信相關(guān)的開發(fā)相對方便，且虛擬儀器的顯示界面十分適合用作開發(fā)上位機(jī)監(jiān)測模塊。

上位機(jī)模塊主要實現(xiàn)如下功能：

（1）監(jiān)測四旋翼飛行器當(dāng)前的PWM、加速度、陀螺儀、磁力計、高度、溫度、壓力等多項參數(shù)，繪制相關(guān)的圖像并保存數(shù)據(jù)，并通過3D圖像實時模擬飛行器姿態(tài)；

（2）對四旋翼飛行的陀螺儀、磁力計等多個傳感器的值進(jìn)行校訂與配置；

（3）讀取、修改四旋翼飛行器的PID參數(shù)；

（4）模擬四旋翼飛行器控制器手柄的實時狀態(tài)；

（5）實時獲取攝像頭采集圖像并保存；

（6）通過PC鍵盤上的按鍵對四旋翼飛行器進(jìn)行簡易控制；

（7）獲取飛行器當(dāng)前在空間中的三維坐標(biāo)、速度以及飛行軌跡；

（8）輸入目的坐標(biāo)，飛行器可自動飛行到該點上。

3 系統(tǒng)實現(xiàn)

在飛行器姿態(tài)解算時，采用數(shù)據(jù)融合算法來減小誤差，部分代碼如下：

norm=invSqrt(ax*ax+ay*ay+az*az);

ax=ax*norm;

ay=ay*norm;

az=az*norm;

norm=invSqrt(mx*mx+my*my+mz*mz);

mx=mx*norm;

my=my*norm;

mz=mz*norm;

hx=2*mx*(0.5f-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2);

hy=2*mx*(q1q2+q0q3)+2*my*(0.5f-q1q1-q3q3)+2*mz*(q2q3-q0q1);

hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5f-q1q1-q2q2);

bx=sqrt((hx*hx)+(hy*hy));

bz=hz;

vx=2*(q1q3-q0q2);vy=2*(q0q1+q2q3);vz=q0q0-q1q1-q2q2+q3q3;

wx=2*bx*(0.5f-q2q2-q3q3)+2*bz*(q1q3-q0q2);

wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);

wz=2*bx*(q0q2+q1q3)+2*bz*(0.5f-q1q1-q2q2);

ex=(ay*vz-az*vy)+(my*wz-mz*wy);

ey=(az*vx-ax*vz)+(mz*wx-mx*wz);

ez=(ax*vy-ay*vx)+(mx*wy-my*wx);

其中 ax、ay、az為加速度計測量值，mx、my、mz為磁力計測量值，vx、vy、vz是陀螺儀測量值積分后，再通過解算得出的重力向量，它們之間的差異就是陀螺儀積分后計算出的姿態(tài)與加速度計和磁力計測出姿態(tài)之間的差異。因為向量間的差異可用向量叉積表示，ex、ey、ez就是兩個重力向量的叉積，該叉積最終用于更新四元數(shù) q0、q1、q2、q3。

Android端主界面如圖8所示：

圖8 Android端主界面

其中標(biāo)記處：①②為操縱桿；③為采集視頻與圖像顯示界面；④為控制信息顯示面板；⑤為傳感器信息顯示面板；⑥為微操模式；⑦為一鍵返航模式；⑧⑨為飛行模式切換按鈕。

Android界面的開發(fā)利用Canvas工具，根據(jù)屏幕的相對位置定位并繪制需要顯示的內(nèi)容。系統(tǒng)將首先計算屏幕的大小，根據(jù)預(yù)先規(guī)定的每個控件所占的百分比相應(yīng)計算出每個控件的實際大小以及在屏幕上的位置。

OFFSETX、OFFSETY、OFFSETX0、OFFSETY0等四個數(shù)據(jù)分別表示左聚焦點在水平、豎直的偏移量和右聚焦點在水平、豎直的偏移量，并根據(jù)這四個數(shù)據(jù)計算符合需要的PWM四個通道的數(shù)據(jù)控制飛行器的姿態(tài)完成起降與飛行。微調(diào)界面主要是為了避免圓形操作區(qū)域過于靈敏造成操作上的不適應(yīng)，方法為降低控件移動時的靈敏度。

當(dāng)處理視頻流時，安卓端從緩存區(qū)中按字節(jié)讀出數(shù)據(jù)，按照預(yù)定協(xié)議來讀取視頻信息，并采用字節(jié)流方式將視頻更新至背景面板。

上位機(jī)程序共包括四個前面板：端口設(shè)置、AHRS數(shù)據(jù)監(jiān)測、虛擬手柄控制端、飛行地圖。

上位機(jī)的運作流程與安卓端類似，在監(jiān)測飛行器當(dāng)前的狀態(tài)時，步驟如下：上位機(jī)先向飛行器發(fā)出數(shù)據(jù)請求；飛行器驗證請求的命令頭；飛行器按照設(shè)計協(xié)議讀取上位機(jī)希望獲得數(shù)據(jù)的ID；飛行器將數(shù)據(jù)發(fā)送給上位機(jī)，上位機(jī)將獲得的數(shù)據(jù)寫入，以圖標(biāo)形式顯示在控件上。

在調(diào)參以及對飛行器進(jìn)行輔助控制時，為保證即時性，上位機(jī)采用LabVIEW中的事件結(jié)構(gòu)。上位機(jī)在下達(dá)飛行任務(wù)或調(diào)整參數(shù)時觸發(fā)事件，通過藍(lán)牙向飛行器發(fā)送數(shù)據(jù)，飛行器驗證命令后作出相應(yīng)的反饋。

4 效果圖

Android端運行效果如圖9所示。

圖9Android端運行界面

系統(tǒng)采用成熟的SurfaceView控件設(shè)計，當(dāng)系統(tǒng)內(nèi)某個數(shù)據(jù)修改時將會對系統(tǒng)界面產(chǎn)生回調(diào)，立即更新界面數(shù)據(jù)。

上位機(jī)端口設(shè)置前面板如圖10所示。

圖10 端口設(shè)置前面板

該面板用于與PC連接前的配置，在連接后可實時顯示各通道PWM值，模擬出當(dāng)前手柄狀態(tài)并根據(jù)當(dāng)前姿態(tài)數(shù)據(jù)模擬機(jī)體的3D圖像。

上位機(jī)AHRS數(shù)據(jù)監(jiān)測前面板如圖11所示。

圖11 AHRS數(shù)據(jù)監(jiān)測前面板

該面板用于監(jiān)測飛行器當(dāng)前狀態(tài)以及周圍環(huán)境信息，包括姿態(tài)角、加速度、高度、溫度等，并將圖表保存至本地，還可根據(jù)數(shù)據(jù)精準(zhǔn)度校正傳感器或修改配置信息。

上位機(jī)虛擬手柄控制端前面板如圖12所示。

該面板用于PC端對于飛行器的輔助控制，可通過鍵盤對飛行器發(fā)布一些簡單飛行指令，避免Android端出現(xiàn)意外導(dǎo)致飛機(jī)失控。同時該界面可實時捕獲機(jī)體載有攝像頭采集的圖片，并可保存至本地.

上位機(jī)飛行地圖前面板如圖13所示。

圖12 虛擬手柄控制端前面板

圖13 飛行地圖前面板

該面板用于發(fā)布定點飛行任務(wù)，在前面板輸出目標(biāo)坐標(biāo)后，飛行器即可前往該目標(biāo)點，并懸停于該點上。前面板可實時讀取飛行器在平面上以及在空間內(nèi)的飛行軌跡。

5 結(jié)語

在本項目研究中，搭建了可以實現(xiàn)一系列任務(wù)功能、有一定抗擾性的四旋翼飛行器，設(shè)計了用于控制飛行器的Android端和上位機(jī)軟件。這一系列的設(shè)計簡化了四旋翼飛行器的操作，并可使用戶更加方便地進(jìn)行監(jiān)測控制。

[1]劉麗麗，四旋翼飛行仿真器的建模及控制方法的研究[D].湖南長沙：中南大學(xué)，2009

[2]PAUL G F,THOMAS J G.Introduction to UAV Systems[M].Columbia,MD:UAV Systems,1998.