陳云飛 陳興武 許超

（福建工程學院電子電氣與物理學院 福建省福州市 350118）

小型無人機的研制與應用范圍突飛猛進，以其擁有良好的靈活性和廉價等特性受到人們的廣泛關注[1]。特別是在2015年無人機元年之后，中國無人機市場蓬勃發(fā)展帶動了安防、軍用、農(nóng)業(yè)和物流產(chǎn)業(yè)，無人機在無人駕駛的條件下進行復雜的空中飛行任務和各類負載任務。在全國抗擊新型冠狀病毒期間，無人機可代替人類完成巡邏和室外消毒等危險且繁瑣的工作，無人機的作用功不可沒，因此在此基礎上提高無人機的穩(wěn)定性和可靠性具有非同尋常的意義。

飛行控制系統(tǒng)是一個六自由度欠驅動系統(tǒng)[2]，具有多變量非線性和強耦合的特性。以姿態(tài)控制為無人機控制的核心，在大部分環(huán)境中使用PID 控制算法擁有較好的控制效果。因此研究適宜多旋翼無人機飛行控制算法十分重要。

民用無人機中大部分的使用場景是航拍，國內的無人機廠商有近一半產(chǎn)品是航拍無人機廠商[3]。在人工智能來臨之際，通過搭載帶有云臺的攝像頭，航拍無人機可以在高空拍攝照片和視頻，包括旅游航拍、三維建模以及視頻錄制。擁有較強的穩(wěn)定性能、續(xù)航能力是航拍無人機的基本要求，目前民用航拍無人機已經(jīng)得到了廣泛的應用。

1 軟硬件設計方案

1.1 硬件方案

國外開源飛控中主要有APM，Pixhawk， FlyingF4 等飛控[3]。其中基于Pixhawk 硬件的arducopter 無人機自動控制系統(tǒng)代碼結構好、便于開發(fā)，是無人機開發(fā)者學習的首選。

本設計采用ST 公司的STM32F427VIT6 作為主控芯片其擁有較多的接口以及較高的主頻受到人們的廣泛使用。該32-bit Cortex-M4 內核芯片比較強大，具有128KB SRAM、 512KB FLASH、5 個SPI 接口、3 個 IIC 接口、1 個 USB 接口、以及 36 個通用IO 口。

基于Pixhawk 飛行控制器設計原理[5]，使用內部集成了3 軸磁力傳感器，3 軸加速度傳感器和3 軸陀螺儀傳感器的MPU6050 芯片測量四旋翼無人機的姿態(tài)數(shù)據(jù)。通過外設接口，結合卡爾曼濾波算法對其數(shù)據(jù)進行精準的解算，從而對無人機的運動狀態(tài)進行信息采集。無人機及傳感器硬件連接圖如圖1所示。

圖1：無人機及傳感器硬件連接圖

1.2 構建開發(fā)環(huán)境

在搭建環(huán)境過程中，使用Windows10 操作系統(tǒng)，CPU 為I7-9700F，顯卡為RTX2060 SUPER，內存為16GB，分配100GB 存儲空間從而保證虛擬機的流暢運行。并搭建虛擬機，具體配置和版本如表1所示。

表1：硬件配置

在環(huán)境搭建過程中，首先通過在Windows10 系統(tǒng)下使用VMware Workstation Pro 軟件運行Ubuntu16.04 版本的虛擬機。在terminal（終端）從github 的arducopter 開源項目上（https://github.com/arducopter/arducopter）下載飛控程序。接著在Ubuntu 系統(tǒng)下的sublime 對已有的程序進行裁剪，編寫。最后對程序進行編譯，并燒錄到無人機飛控中。

2 飛控程序設計

2.1 源碼與軟件初始化

Arducopter 源碼由各種飛行器的主要控制代碼：如旋翼機，固定翼，智能車；共享的庫文件：包括姿態(tài)解算的庫函數(shù)，相關控制算法（PID）的函數(shù)；硬件抽象層（方便arducopter 移植不同的硬件平臺）；示例代碼和文件說明和擴展接口的硬件函數(shù)庫組成。

在分析arducopter 代碼要從arducopter.cpp 和setup()函數(shù)著手理解。在arducopter.cpp 文件下使用loop()函數(shù)的設計框架既準確又高效。整體設計思路：首先用計時器定時觸發(fā)測量；其次全部測量過程都靠中斷推進；最后在main()函數(shù)里不斷檢查測量是否完畢。在setup()函數(shù)內部通過調用init_arducopter()函數(shù)，該函數(shù)做了一系列的初始化，以此來保障無人機在啟動時可以通過自檢。當setup()函數(shù)完成自檢功能之后，在上位機也就是地面站系統(tǒng)中會提示“Ready to fly”字樣。此時四旋翼無人機進入隨時可以遙控起飛的狀態(tài)。

2.2 程序框架與算法

常見的無人機傳感器有姿態(tài)傳感器，氣壓高度傳感器和定位模塊（GPS）組成[6]。使用到氣壓傳感器MS5611 和姿態(tài)傳感器MPU6050。傳感器驅動程序在后端收集原始姿態(tài)數(shù)據(jù)并轉換為標準單位的數(shù)據(jù)，保存到驅動器的緩存區(qū)當中，發(fā)送給擴展卡爾曼濾波（EKF）模塊。

arducopter 控制的流程首先經(jīng)過loop()函數(shù)后，卡爾曼濾波（EKF）模塊處理傳感器，加速度，陀螺儀，指南針等信息。再通過執(zhí)行模式程序、控制程序、位置控制程序、姿態(tài)控制程序，最后將PWM 信號發(fā)送到硬件抽象層的電機和舵機等設備。

卡爾曼濾波（Kalman filtering）是利用線性狀態(tài)方程，通過測算和比對傳感器發(fā)送的姿態(tài)數(shù)據(jù)，對無人機姿態(tài)進行有效估計的算法。簡單的說卡爾曼濾波的算法是對傳感器狀態(tài)更新（數(shù)據(jù)取加權平均）并不斷進行時間更新（迭代）。主要應用于減少傳感器噪聲對飛行器實際飛行狀態(tài)的干擾。

在常見的控制系統(tǒng)中，通過比例、積分和微分進行控制的PID控制器是當前應用最常見且可靠的自動控制器。PID 控制具有適用面廣，公式原理簡單，易于實現(xiàn)，各控制參數(shù)較為獨立等優(yōu)點。PID 控制的公式如下所示。

串級PID 控制通過測量角度和角速度，在內環(huán)使用角速度環(huán)，外環(huán)使用角度環(huán)。程序設計中AC_PosControl 類可以把XY 軸的位置P 將位置誤差轉換為目標速度，速度PID 信號將速度誤差轉換為所需的加速度信號，然后將其轉換為所需的傾斜角度，最后將其發(fā)送到姿態(tài)控制庫并被調用。也就是可以在無人機還沒有產(chǎn)生較大位置偏差和角度偏差時，就及時的糾正無人機的姿態(tài)，使飛行器的穩(wěn)定性更高，適應能力更強。用目標角度與實際角度得出的姿態(tài)偏差量作為輸入，通過比例調節(jié)器將目標轉化為實際的角速度，接著將誤差通過PID 控制求和發(fā)送到電機，從而達到調節(jié)無人機姿態(tài)。使用PID 控制算法，可以先調節(jié)P 控制器再組成PI 調節(jié)器，最后組成PID 調節(jié)器。在各環(huán)節(jié)也可按照角度環(huán)PID，角速度環(huán)PID，Z軸高度環(huán)PID 控制無人機的飛行姿態(tài)，有順序的進行調節(jié)，使無人機可以穩(wěn)定的懸停。

圖3：無人機懸停控制框圖

Arducopter 飛行姿態(tài)控制程序結構如下（附圖如圖2所示）：

圖2：arducopter 姿態(tài)控制流程圖

（1）調用“Flight-mode.cpp”中“update_flight_mode()”函數(shù)，檢查載具的種類是否屬于copter（旋翼機）。

（2）調用“Mode_stabilize.cpp”中“run()”函數(shù)，將傳感器采集到的信息轉化為橫滾，俯仰和偏航角度等參數(shù)。

（3） 調用“AC_attitudeControl.cpp” 中“input_euler_anger_rool_pitch_rate_yaw()”和“set_ throttle.out()”函數(shù)，計算姿態(tài)誤差并轉換為橫滾、俯仰、偏航旋轉速率并計算轉速誤差并轉換為電機給定信號。

（4）調用“AP_ MotorsMatix.cpp”中的“output()”函數(shù)，將整體的電機信號發(fā)送給各個電機。

（5）調用“SRV_ Channels.cpp”中的“push()”函數(shù)，將信號按照比例縮放后發(fā)送。

（6）調用“AP_HAL:RCOutput.cpp”中的“push()”函數(shù)，將飛控發(fā)出的控制信號轉換成PWM 波的形式發(fā)送給電調，電調控制電機旋轉。

程序的循環(huán)執(zhí)行，通過控制飛行姿態(tài)從而控制無人機在空中懸停。

基于ardupilot 控制無人機懸停有兩種方式分別是Loiter 模式和Poshold 模式，兩種方式差別很大，Loiter 模式控制無人機的運動加速度來實現(xiàn)位置控制；PosHold 模式和Loiter 模式類似，都是保持當前高度、位置和航向，不同的是PosHold 模式的搖桿直接控制飛行器偏轉的角度。以Loitor 模式進行說明，無人機供電啟動后首先進行初始化進行自檢程序，其次當需要控制無人機位置時需要獲取GPS 和氣壓計MS5611 的傳感器信息，當需要控制高度時需要獲取陀螺儀MPU6050 和氣壓計MS5611 的傳感器信息并進行卡爾曼濾波，之后飛控處理器獲取到起飛前的位置高度信息并獲取到遙控器的期望值，通過與實際值對比，通過串級PID 控制從而及時的糾正無人機的姿態(tài)，最后將PWM 信號輸出到電調模塊，控制電機轉速從而使飛行器的穩(wěn)定性更高，適應能力更強。

3 測試與實驗

硬件平臺使用Pixhawk2.4.8 飛控模塊，燒錄優(yōu)化后的程序，搭配X360 碳纖維機架、好盈30A 電調4 個、格式4S 3300mah 鋰離子電池、2212 電機及配套的9045 電動螺旋槳，驗證程序設計的合理性。

首先使用micro-USB 數(shù)據(jù)線將飛控與電腦相連接，在安裝飛控相關組件和STM32 虛擬串口的程序后，電腦設備管理器中可以找到COM5 端口，此時可以進行地面站的軟件測試。

QGroundControl 地面站可以為無人機提供完整的飛行控制和任務計劃，其代碼的開源性，方便了開發(fā)者的使用。利用此軟件在本課題中可以監(jiān)控飛控的傳感器數(shù)據(jù)并進行程序的燒錄。

通過QGroundControl 中的監(jiān)控按鈕MAVlink Inspector，可以檢查各個傳感器的當前姿態(tài)，并繪制出折線圖。當調整YAW 軸時，YAW 和YAWSPEED 變化較為明顯，且YAWSPEED 大小決定了YAW 軸曲線的變化速率。同樣MS5611 氣壓傳感器快速地將飛控從高處放到低處再返回到高處，也可以看到地面站上出現(xiàn)一條V 形曲線。使用MPU6000 六軸陀螺儀對陀螺儀參數(shù)進行檢測。通過對x 軸進行旋轉，可以得到xgyro 在圖像上有較大的波動，而ygyro和zgyro 的變化幅度較小。同樣測試加速度傳感器，在短時間內迅速改變x 軸加速度，即可在圖像上明顯地觀測到綠色折線有較大波動。在測試飛行過程中，無人機懸停姿態(tài)良好，沒有異常抖動。在給予一個較大的俯仰舵量時，依然可以保持理想的飛行姿態(tài)和路線，實現(xiàn)了無人機低空懸停。

4 總結與展望

本文研究的是嵌入式系統(tǒng)中無人機低空懸?？刂瞥绦蛟O計，基于已有的開源平臺對低空懸停的無人機進行特定的內核裁剪和程序設計。包括采集無人機多種傳感器信號并進行數(shù)據(jù)融合，實現(xiàn)無人機角速度、加速度、航向相對于地面高度的獲取。解算出的俯仰橫滾及橫向角度信息通過嵌入式系統(tǒng)的飛控處理，使用串級PID 控制算法控制無人機實現(xiàn)無人機低空懸停。