基于Android的多功能四旋翼飛行器設計

巫子晨，楊文韜，孫蓬勃

（大連理工大學軟件學院，大連116621）

1 需求分析

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

基于以上分析，在控制端應用方面，需要一個簡潔友好的控制界面方便用戶控制，例如運用手機App直接對飛行器進行控制，以替代傳統的遙控器。或是直接通過PC端應用程序控制，在控制端實時獲取飛行器的狀態參數，發布飛行器任務，例如路線規劃，自動投遞等。在飛行器設計方面，要具有一定的抗干擾能力和較高的自動化程度，以方便用戶使用，例如自動起飛和降落、自動定高和定點，當受到外界干擾時，能通過自我調節保持穩定等。

2 系統設計

本系統的主要工作邏輯如下：四旋翼飛行器啟動，按照設計程序執行飛行任務；Android App通過Wi-Fi連接四旋翼飛行器，監測并控制飛行器當前狀態；上位機通過藍牙連接至飛行器，輔助控制飛行器。系統結構層次見圖1：本系統的主要功能模塊劃分為：目標機模塊、Android端模塊和上位機模塊。其中以四旋翼飛行器為核心，Android端與上位機則負責監測飛行器狀態并施以輔助控制。

2.1 目標機模塊設計

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

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

圖2 四軸軟件與硬件兩方面內容

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

（1）傳感器的數據融合算法

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

設計的互補濾波器的傳遞函數為：

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

互補濾波姿態解算的步驟如圖3所示。

圖3 互補濾波姿態解算步驟圖

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

（2）飛行控制算法

四旋翼飛行器的控制目標是使其在無動作指令時保持穩定懸停狀態，有動作指令時有效完成指定動作。串級PID控制器結構簡單、穩定性好、工作可靠、調整方便，在工業控制中被廣泛使用。使用串級PID控制器進行飛行器的姿態控制，原理如圖4所示：

圖4 串級PID原理圖

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

圖5 飛行姿態PID控制器原理圖

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

2.2 Android端模塊

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

（1）界面設計

本系統的Android App界面布局如圖6：

圖6 Android App界面布局設計

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

（2）通信協議設計

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

圖7 部分通信協議

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

2.3 上位機模塊

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

上位機模塊主要實現如下功能：

（1）監測四旋翼飛行器當前的PWM、加速度、陀螺儀、磁力計、高度、溫度、壓力等多項參數，繪制相關的圖像并保存數據，并通過3D圖像實時模擬飛行器姿態；

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

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

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

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

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

（7）獲取飛行器當前在空間中的三維坐標、速度以及飛行軌跡；

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

3 系統實現

在飛行器姿態解算時，采用數據融合算法來減小誤差，部分代碼如下：

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是陀螺儀測量值積分后，再通過解算得出的重力向量，它們之間的差異就是陀螺儀積分后計算出的姿態與加速度計和磁力計測出姿態之間的差異。因為向量間的差異可用向量叉積表示，ex、ey、ez就是兩個重力向量的叉積，該叉積最終用于更新四元數 q0、q1、q2、q3。

Android端主界面如圖8所示：

圖8 Android端主界面

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

Android界面的開發利用Canvas工具，根據屏幕的相對位置定位并繪制需要顯示的內容。系統將首先計算屏幕的大小，根據預先規定的每個控件所占的百分比相應計算出每個控件的實際大小以及在屏幕上的位置。

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

當處理視頻流時，安卓端從緩存區中按字節讀出數據，按照預定協議來讀取視頻信息，并采用字節流方式將視頻更新至背景面板。

上位機程序共包括四個前面板：端口設置、AHRS數據監測、虛擬手柄控制端、飛行地圖。

上位機的運作流程與安卓端類似，在監測飛行器當前的狀態時，步驟如下：上位機先向飛行器發出數據請求；飛行器驗證請求的命令頭；飛行器按照設計協議讀取上位機希望獲得數據的ID；飛行器將數據發送給上位機，上位機將獲得的數據寫入，以圖標形式顯示在控件上。

在調參以及對飛行器進行輔助控制時，為保證即時性，上位機采用LabVIEW中的事件結構。上位機在下達飛行任務或調整參數時觸發事件，通過藍牙向飛行器發送數據，飛行器驗證命令后作出相應的反饋。

4 效果圖

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

圖9Android端運行界面

系統采用成熟的SurfaceView控件設計，當系統內某個數據修改時將會對系統界面產生回調，立即更新界面數據。

上位機端口設置前面板如圖10所示。

圖10 端口設置前面板

該面板用于與PC連接前的配置，在連接后可實時顯示各通道PWM值，模擬出當前手柄狀態并根據當前姿態數據模擬機體的3D圖像。

上位機AHRS數據監測前面板如圖11所示。

圖11 AHRS數據監測前面板

該面板用于監測飛行器當前狀態以及周圍環境信息，包括姿態角、加速度、高度、溫度等，并將圖表保存至本地，還可根據數據精準度校正傳感器或修改配置信息。

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

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

上位機飛行地圖前面板如圖13所示。

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

圖13 飛行地圖前面板

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

5 結語

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

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

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