基于STM32的目標(biāo)跟蹤無人機(jī)整體及飛控系統(tǒng)控制算法設(shè)計(jì)與研究

陳斌 施偉 原鑫鑫 馬原

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

關(guān)鍵詞：STM32;飛控系統(tǒng);實(shí)時(shí)跟蹤;軟硬件設(shè)計(jì)

中圖分類號(hào)：TP212 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1007-9416（2019）12-0175-04

0 引言

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

1 系統(tǒng)硬件設(shè)計(jì)

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

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

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

2.1 設(shè)計(jì)原則及流程

本次作品的軟件設(shè)計(jì)主要分為兩部分：飛控模塊的軟件設(shè)計(jì)和目標(biāo)實(shí)時(shí)跟蹤的軟件設(shè)計(jì)。飛控模塊是基于STM32F407ZGT6微處理器的軟件程序設(shè)計(jì)，目標(biāo)實(shí)時(shí)跟蹤的軟件設(shè)計(jì)是應(yīng)用機(jī)器學(xué)習(xí)算法中改進(jìn)的KCF算法。

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

無人機(jī)的飛控軟件運(yùn)行于STM32F407ZGT6硬件電路，基于ST標(biāo)準(zhǔn)外設(shè)庫開發(fā)。

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

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

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

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

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

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

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

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

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

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

由載體坐標(biāo)系到導(dǎo)航坐標(biāo)系轉(zhuǎn)換的姿態(tài)矩陣為：

（1）

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

（2）

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

（3）

（4）

2.3 實(shí)時(shí)控制算法設(shè)計(jì)

在動(dòng)力學(xué)模型的基礎(chǔ)上，將四旋翼無人機(jī)實(shí)時(shí)控制算法分為三個(gè)PID控制回路，即姿態(tài)控制回路、高度控制回路和位置控制回路。

2.3.1 姿態(tài)控制回路設(shè)計(jì)

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

2.3.2 高度控制回路設(shè)計(jì)

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

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

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

（5）

式中：

：當(dāng)前時(shí)刻融合得到的值

：上一時(shí)刻融合得到的值

：當(dāng)前時(shí)刻高度測(cè)量值

：變化量

：計(jì)算權(quán)重

：計(jì)算時(shí)間間隔

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

2.3.3 位置控制回路設(shè)計(jì)

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

（6）

（7）

式中：

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

3 系統(tǒng)動(dòng)態(tài)飛行實(shí)驗(yàn)測(cè)試

在進(jìn)行動(dòng)態(tài)飛行測(cè)試的時(shí)候要對(duì)無人機(jī)的電源線，電調(diào)線焊錫處進(jìn)行檢查，檢查電池容量及各接線情況，同時(shí)確保槳安裝的正確。做好相應(yīng)的準(zhǔn)備工作。在進(jìn)行電調(diào)飛行實(shí)驗(yàn)時(shí)，選擇空曠無人且有草坪的地方。

3.1 飛行測(cè)試

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

3.2 翻滾測(cè)試

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

3.3 俯仰測(cè)試

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

4 結(jié)語

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

參考文獻(xiàn)

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

[2] 楊光祥.STM32單片機(jī)原理與工程實(shí)踐[M].武漢：武漢理工大學(xué)出版社，2013.

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

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

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

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

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

[8] 李海泉.小型無人機(jī)飛行力學(xué)建模及虛擬訓(xùn)練平臺(tái)的建立[D].南京：南京航空航天大學(xué)，2012.

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

[10] 胡占雙.無人機(jī)飛行姿態(tài)檢測(cè)及控制研究[D].沈陽：沈陽航空航天大學(xué)，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