陳斌 施偉 原鑫鑫 馬原

摘要：本次設計依據(jù)四旋翼無人機實際運行測試中的需求，論述了一種基于STM32的目標跟蹤無人機整體及飛控系統(tǒng)控制算法設計方案，包括系統(tǒng)軟硬件設計、飛控系統(tǒng)控制算法設計、系統(tǒng)動態(tài)飛行實驗測試等方面。測試結果表明系統(tǒng)可通過各個模塊的配合實現(xiàn)對飛控及電機的精確控制，具有相應速度快、定位誤差小、運行較為穩(wěn)定等特點。

關鍵詞：STM32;飛控系統(tǒng);實時跟蹤;軟硬件設計

中圖分類號：TP212 文獻標識碼：A 文章編號：1007-9416（2019）12-0175-04

0 引言

在人工智能和中國制造2025的大背景下，智能無人機已經(jīng)從概念化走向商業(yè)化，市場份額不斷擴大，發(fā)展迅速，國內無人機技術已經(jīng)取得了部分成果，但與發(fā)達國家相比較仍具有差距，未來無人機可為影視跟蹤拍攝、足球賽事判罰和瀕危動物生活習性監(jiān)控等提供了一個新的解決方案，智能無人機的應用市場前景很廣闊。在這些應用領域中，對無人機提出了小型化、長航時等新的性能要求，面對這些新需求，無人機的整體架構及飛控系統(tǒng)也需要進行相應的改進設計，以滿足無人機飛行時的控制性能要求，因此，對于小型無人機整體及飛控系統(tǒng)的設計與研究將對無人機的進一步應用具有重要的意義。

1 系統(tǒng)硬件設計

為了實現(xiàn)完善的姿態(tài)控制，需要多傳感器參與控制反饋，由主控制器收集信息并綜合所有傳感器信息產(chǎn)生控制量，將控制量傳遞給電機調速器，從而實現(xiàn)對無人機姿態(tài)、位置、高度的控制。本次設計根據(jù)四旋翼飛行器飛行原理，建立系統(tǒng)動力學模型，選用飛越650機架作為機體，通過好盈樂天40A電子調速器控制4個無刷電機轉速來實現(xiàn)無人機的欠驅動系統(tǒng)飛行。系統(tǒng)以STMF407作為四旋翼自主無人機控制的核心，由鋰電池模塊、電機調速控制模塊、傳感器檢測模塊、飛控模塊等構成。飛行控制模塊包括角度傳感器（陀螺儀和加速度計）、磁力計、氣壓計，傳感器檢測模塊包括TOF線性激光傳感器、光流模塊、PMU電源管理模塊。此外，選取Jetson TX2嵌入式開發(fā)板應用改進的KCF算法在該部件上進行實時地目標跟蹤，如圖1所示為無人機控制系統(tǒng)硬件總體方案。

2 系統(tǒng)軟件設計

本次軟件系統(tǒng)設計所采用的STM32F407ZGT6主控芯片軟件開發(fā)環(huán)境在MDK-ARM 5.15開發(fā)環(huán)境下以C與C++混合編程，并配置相應的外設實現(xiàn)姿態(tài)解算、運動控制。

2.1 設計原則及流程

本次作品的軟件設計主要分為兩部分：飛控模塊的軟件設計和目標實時跟蹤的軟件設計。飛控模塊是基于STM32F407ZGT6微處理器的軟件程序設計，目標實時跟蹤的軟件設計是應用機器學習算法中改進的KCF算法。

首先對飛控模塊進行初始化，單片機進入定時器中斷循環(huán)，STM32F407ZGT6單片機采用icm20602傳感器通過SPI協(xié)議讀取icm20602中的陀螺儀、加速度計數(shù)據(jù)，每2ms進行一次姿態(tài)解算，得到歐拉角，每4ms進行一次單片機的控制運算，輸出最終的PWM值，進而完成飛行姿態(tài)控制回路的設計，使無人機穩(wěn)定飛行;通過單片機獲取線性激光傳感器和氣壓計的高度數(shù)據(jù)，將兩者的數(shù)據(jù)進行融合后，每隔10ms進行一次高度環(huán)的計算，從而完成飛行高度控制回路的設計;單片機讀取光流傳感器和Jetson TX2通過串口發(fā)送的目標位置信息數(shù)據(jù)，每隔20ms將兩者數(shù)據(jù)進行互補融合運算，從而完成飛行位置控制回路的設計。如圖2所示為四旋翼無人機控制系統(tǒng)的總體方案。

無人機的飛控軟件運行于STM32F407ZGT6硬件電路，基于ST標準外設庫開發(fā)。

主要實現(xiàn)下列功能：

（1）讀取傳感器信息：通過串口讀取姿態(tài)傳感器數(shù)據(jù)信息;通過串口讀取光流傳感器與TOF線性激光傳感器信息;通過SPI讀取氣壓計信息。

（2）接收機信號收?。哼b控機接收機采用高電平的時間長度來承載信息，根據(jù)接收機品牌不同，更新頻率從50Hz到500Hz不等，高電平時間長度通常為1ms～2ms。利用外部中斷對高電平時間進行測量，從而獲取接收機信息。

（3）控制電機調速器與云臺控制器：電機調速器與云臺控制器均接受與接收機相類似的數(shù)字信號，用高電平時間長短來判斷控制量的大小。采用內部定時器產(chǎn)生4路PWM波形用于控制六個電機轉速，通過2路PWM波形實現(xiàn)對于云臺俯仰、橫滾的角度控制。

（4）與地面站通訊：通過數(shù)傳模塊進行雙向通訊。打包機上的飛行信息發(fā)送給地面站，接收地面站傳來的指令并正確執(zhí)行。

（5）數(shù)據(jù)濾波：運用數(shù)字濾波方法對傳感器測得的數(shù)據(jù)進行處理，主要目的是降低信號中的噪聲水平，排除震動等因素對于傳感器造成的影響。

（6）控制量解算：根據(jù)反饋與給定之間的偏差，計算位置、速度、高度、爬升速度、姿態(tài)角、姿態(tài)角速度等控制量。

2.2 基于EKF姿態(tài)解算算法設計

在無人機控制系統(tǒng)中，姿態(tài)信息是最基本的導航參數(shù)，大多的控制算法都是以姿態(tài)信息的準確性為前提的。本次設計采用擴展卡爾曼濾波器（EKF）來對速率陀螺、加速度計和磁力計的數(shù)據(jù)融合得到優(yōu)化的姿態(tài)信息。

在導航坐標系的原點選在無人機重心處，取軸指向北，軸指向東，軸沿垂線方向指向天，即東北天坐標系。在載體坐標系的原點位于無人機重心處，沿機體橫軸指向右，沿機體縱軸指向前，沿機體的豎軸指向上，滿足右手定則。、、分別為無人機的航向角，俯仰角和姿態(tài)角。

由載體坐標系到導航坐標系轉換的姿態(tài)矩陣為：

（1）

由四元數(shù)來表示的姿態(tài)矩陣為：

（2）

根據(jù)姿態(tài)矩陣的元素，可以確定（、、）的值，其中，和需要根據(jù)公式（3）對主值進行判斷。

（3）

（4）

2.3 實時控制算法設計

在動力學模型的基礎上，將四旋翼無人機實時控制算法分為三個PID控制回路，即姿態(tài)控制回路、高度控制回路和位置控制回路。

2.3.1 姿態(tài)控制回路設計

當四軸無人機正常飛行時，突遇外力（風等）或磁場干擾，使加速度傳感器或磁力傳感器采集數(shù)據(jù)失真，造成姿態(tài)解算出來的歐拉角錯誤，只用角度單環(huán)情況下，使系統(tǒng)很難穩(wěn)定運行，因此可以加入角速度作為內環(huán)，角速度由陀螺儀采集數(shù)據(jù)輸出，采集值一般不存在受外界影響情況，抗干擾能力強，并且角速度變化靈敏，當受外界干擾時，回復迅速，姿態(tài)控制回路如圖3所示。

2.3.2 高度控制回路設計

高度控制是指通過改變四旋翼無人機的總升力，使得無人機的飛行高度達到并維持在給定高度的過程。高度控制的難點在于準確地獲取高度信息。高度控制通過TOF線性激光傳感器、加速度計、氣壓計數(shù)據(jù)進行數(shù)據(jù)融合得到數(shù)據(jù)，從而獲取高度反饋。其中，TOF線性激光傳感器具有極佳的測距精度，且不受氣流的影響，但是只能進行短距離測距;氣壓計測距范圍不受限制，且可以提供極佳的相對高度信息，但是氣壓計存在漂移現(xiàn)象，隨著溫度或陣風，其輸出均會受到一定的影響;加速度計由于機體震動的影響，輸出量中的噪聲部分比較高，但是加速度計具有高動態(tài)與高靈敏的特點，可以對低頻數(shù)據(jù)（如10Hz的超聲波數(shù)據(jù)）進行插值計算，使低頻數(shù)據(jù)也可以獲取高動態(tài)的輸出特性，高度控制回路設計如圖4所示。

由于TOF線性激光傳感器與氣壓計均有測量高度的功能，而加速度計可以測出機體豎直方向的加速度，因此，可以融合TOF線性激光傳感器、氣壓計與加速度計的輸出結果得到高度估計值。氣壓計用于高空高度數(shù)據(jù)的測量，TOF線性激光用于低空高度數(shù)據(jù)的測量。無人機在高空和低空飛行時高度數(shù)據(jù)的測量可以智能切換。

首先利用所測得高度數(shù)據(jù)與加速度計測出機體豎直方向的加速度進行一階互補融合得到一個對地速度的估計值，然后利用該對地速度的估計值與氣壓計原始數(shù)據(jù)進行一階互補融合得到最終的高度估計值，一階互補融合根據(jù)如下式子計算：

（5）

式中：

：當前時刻融合得到的值

：上一時刻融合得到的值

：當前時刻高度測量值

：變化量

：計算權重

：計算時間間隔

在實際計算中可以根據(jù)加速度計和氣壓計的可信賴程度調整計算權重，從而使估計值更快的收斂。通過兩次互補融合，可以得到一個比較可靠的對地速度的估計值和高度估計值。

2.3.3 位置控制回路設計

目標水平位置信息通過串口發(fā)送給飛控模塊，通過飛控模塊讀取光流傳感器對應的水平速率，對水平速率v（t）通過公式（6）得到水平位移s（t），將計算的水平位移s（t）和經(jīng)過跟蹤算法計算出的目標水平位置信息x在飛控模塊中運用公式（7）進行互補融合，得到精確的位置信息y，將該結果作為位置環(huán)的外環(huán)控制回路，將通過飛控模塊讀取光流傳感器對應的水平速率作為位置環(huán)的內環(huán)控制回路，進而完成位置控制回路的設計，如圖5所示。

（6）

（7）

式中：

a：互補融合系數(shù)，取值區(qū)間[0，1]

3 系統(tǒng)動態(tài)飛行實驗測試

在進行動態(tài)飛行測試的時候要對無人機的電源線，電調線焊錫處進行檢查，檢查電池容量及各接線情況，同時確保槳安裝的正確。做好相應的準備工作。在進行電調飛行實驗時，選擇空曠無人且有草坪的地方。

3.1 飛行測試

將無人機置于水平地面，插電源插頭，上電，操作者遠離無人機一段距離，油門桿呈內八字（或外八字）形，看到LED燈慢閃，表示飛控已經(jīng)解鎖，之后慢慢提高油門，將無人機飛到適當高度，然后保持當前油門不動，理論上這時候無人機的升力和重力相等，無人機實現(xiàn)懸停。如圖6無人機定點飛行現(xiàn)場測試圖。

3.2 翻滾測試

推動油門，無人機爬升，保持合適高度，將無人機往左飛行一段，回中保存，再將無人機往右飛行一段，回中保存，對應的翻滾曲線如圖7所示。

3.3 俯仰測試

推動油門，無人機將慢速上升，撥動遙控器方向桿，將無人機往前飛行一段然后回中保存，往后方飛行一段距離，對應的俯仰測試曲線如圖8所示。

4 結語

本次設計依據(jù)四旋翼無人機實際運行測試中的需求，論述了基于STM32的目標跟蹤無人機整體及飛控系統(tǒng)控制算法設計與研究，結合小型無人機的飛行特點和研發(fā)條件，對設計流程進行優(yōu)化，并對EKF姿態(tài)解算和實時控制算法進行設計，在動力學模型的基礎上，將無人機實時控制算法分為三個PID控制回路，即姿態(tài)控制回路、高度控制回路和位置控制回路。并完成系統(tǒng)動態(tài)飛行試驗測試驗證。測試結果表明系統(tǒng)可通過各個模塊的配合實現(xiàn)對飛控及電機的精確控制，具有相應速度快、定位誤差小、運行較為穩(wěn)定等特點。

參考文獻

[1] 劉煥曄.小型四旋翼飛行器飛行控制系統(tǒng)研究與設計[D].上海：上海交通大學，2009.

[2] 楊光祥.STM32單片機原理與工程實踐[M].武漢：武漢理工大學出版社，2013.

[3] 李寧.基于MDK的STM32處理器開發(fā)應用[M].北京：北京航空航天大學出版社，2008.

[4] 單海燕.四旋翼無人直升機飛行控制技術研究[D].南京：南京航空航天大學，2008.

[5] 王永.反電動勢無刷直流電機控制系統(tǒng)研究[D].南京：東南大學，2004.

[6] 姚元鵬.四旋翼直升機控制問題研究[D].哈爾濱：哈爾濱工業(yè)大學，2007.

[7] 蘆勤桑.無人機遙測遙控地面站系統(tǒng)研究[D].南昌：南昌航空大學，2012.

[8] 李海泉.小型無人機飛行力學建模及虛擬訓練平臺的建立[D].南京：南京航空航天大學，2012.

[9] 尚何章.小型無人機飛行控制系統(tǒng)硬件設計與實現(xiàn)[D].南京：南京航空航天大學，2012.

[10] 胡占雙.無人機飛行姿態(tài)檢測及控制研究[D].沈陽：沈陽航空航天大學，2013.

Design and Research of? STM32 Target Tracking UAV Integrated and Flight Control System Control Algorithm

CHEN Bin1，SHI Wei2，YUAN Xin-xin1，MA Yuan1

（1.School of? Computer Science and Information Engineering， Shanghai University of? Applied Sciences， Shanghai? 201400;

2.School of? Automotive Engineering， Jiangsu Automobile Technician College， Yangzhou? Jiangsu? 225000）

Abstract：Based on the requirements in the actual test of the quadrotor drone， this design discussed a STM32-based target tracking drone overall and flight control system control algorithm design scheme， including system software and hardware design， flight control system control algorithm design， system dynamic flight test and other aspects. The test results showed that the system could achieve precise control of flight control and motor through the cooperation of various modules， which had the characteristics of fast speed， small positioning error， and stable operation.

Key words：STM32; Flight control system; real-time tracking; software and hardware design