基于STM32四軸飛行平臺(tái)

易 凡， 鄧 方，張培健，滕璇璇

（武漢輕工大學(xué) 電氣與電子工程學(xué)院，湖北 武漢430040）

基于STM32四軸飛行平臺(tái)

易 凡， 鄧 方，張培健，滕璇璇

（武漢輕工大學(xué) 電氣與電子工程學(xué)院，湖北 武漢430040）

本設(shè)計(jì)以STM32F407為控制核心，四軸飛行器為載體。硬件上由飛控電路，電源管理，通信模塊，動(dòng)力系統(tǒng)，機(jī)架，云臺(tái)伺服系統(tǒng)組成。算法上采用簡(jiǎn)潔穩(wěn)定的四元數(shù)加互補(bǔ)濾波作為姿態(tài)解算算法，PID作為控制器，實(shí)現(xiàn)飛行，云臺(tái)增穩(wěn)等功能。具有靈活輕盈，延展性，適應(yīng)性強(qiáng)好等特點(diǎn)。

四旋翼飛行器；STM32；捷聯(lián)式慣性導(dǎo)航；四元數(shù)姿態(tài)解算

四軸飛行器是一種利用4個(gè)旋翼作為飛行引擎來(lái)進(jìn)行空中飛行的飛行器。進(jìn)入20世紀(jì)以來(lái)，電子技術(shù)飛速發(fā)展四軸飛行器開(kāi)始走向小型化，并融入了人工智能，使其發(fā)展趨于無(wú)人機(jī)，智能機(jī)器人。在實(shí)際應(yīng)用方面，四軸飛行器可以在復(fù)雜、危險(xiǎn)的環(huán)境下可以完成特定的飛行任務(wù)，也可以用于監(jiān)控交通，環(huán)境等。本設(shè)計(jì)主要研究了四軸飛行器的姿態(tài)結(jié)算和飛行控制，并設(shè)計(jì)制作了一架四軸飛行器，對(duì)關(guān)鍵傳感器做了標(biāo)定，并利用用matlab分析數(shù)據(jù)，設(shè)計(jì)算法，還進(jìn)行了單通道平衡試驗(yàn)調(diào)試，進(jìn)行試飛實(shí)驗(yàn)取得了一定的效果。

1 系統(tǒng)方案

本設(shè)計(jì)采用STM32F4作為核心處理器，該處理器內(nèi)核架構(gòu)ARM Cortex-M4，具有高性能、低成本、低功耗等特點(diǎn)。主控板包括傳感器MPU6050電路模塊、無(wú)線(xiàn)藍(lán)牙模塊、電機(jī)啟動(dòng)模塊，電源管理模塊等；遙控使用商品遙控及接收機(jī)?？刂菩酒东@接收機(jī)的PPM命令信號(hào)，傳感器與控制芯片之間采用IIC總線(xiàn)連接，MCU與電調(diào)之間用PWM傳遞控制信號(hào)。軟件算法才用基于四元數(shù)的互補(bǔ)濾波解算姿態(tài)角，控制算法采用經(jīng)典PID控制器控制云臺(tái)舵機(jī)和四軸電機(jī)。圖1為本設(shè)計(jì)總體框圖。

圖1 系統(tǒng)總體框圖

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

針對(duì)前面提出的整體設(shè)計(jì)方案，本設(shè)計(jì)采取模塊化策略，將各個(gè)功能部分開(kāi)來(lái)設(shè)計(jì)，最后組合起來(lái)。

表1 四軸飛行器硬件清單

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

軟件設(shè)計(jì)上由控制核心STM32F4讀取傳感器信息，解算姿態(tài)角，以姿態(tài)角為被控制量融合遙控信息后，輸出到4個(gè)電機(jī)及兩個(gè)舵機(jī)以完成四軸飛行控制和云臺(tái)的穩(wěn)定補(bǔ)償。下圖是軟件流程：

圖2 系統(tǒng)軟件流程

3.1 四元素計(jì)算姿態(tài)角的實(shí)現(xiàn)

根據(jù)前面給出的姿態(tài)解算方程與四元數(shù)，即可得到姿態(tài)計(jì)算系統(tǒng)的計(jì)算原理如下圖。

圖3 姿態(tài)計(jì)算系統(tǒng)計(jì)算原理圖

本設(shè)計(jì)基于互補(bǔ)濾波的思想上完成的四元素算法，其核心思路為利用加速度測(cè)得的重力向量與估計(jì)姿態(tài)得到重力向量的誤差來(lái)矯正陀螺儀積分誤差，然后利用矯正后的陀螺儀積分得到姿態(tài)角。

首先不妨設(shè)處理后的加速度數(shù)據(jù)為：ax，ay，az，單位 m/s^2。加速度計(jì)的向量為陀螺儀數(shù)據(jù)為：gx，gy，gz，單位 rad/s。 陀螺儀向量，由載體到導(dǎo)航坐標(biāo)系的四元數(shù)形式轉(zhuǎn)換矩陣為：。根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量，轉(zhuǎn)到機(jī)體坐標(biāo)系，是中的第三列的3個(gè)元素。所以加速的向量與估計(jì)重力向量叉積：

然后利用向量的叉積，e→可視為誤差向量，這個(gè)叉積向量仍舊是位于機(jī)體坐標(biāo)系上的，而陀螺積分誤差也是在機(jī)體坐標(biāo)系，而且叉積的大小與陀螺積分誤差成正比，正好拿來(lái)糾正陀螺。由于陀螺是對(duì)機(jī)體直接積分，所以對(duì)陀螺的糾正量會(huì)直接體現(xiàn)在對(duì)機(jī)體坐標(biāo)系的糾正。用上面得到的結(jié)果校正陀螺儀：

此處k為一個(gè)常量系數(shù)。再利用二階畢卡法解四元數(shù)微分方程，更新四元數(shù)為下一次計(jì)算做準(zhǔn)備。畢卡二階算法為：

其中Δθ為角增量

最后將四元數(shù)轉(zhuǎn)變?yōu)闅W拉角：

Q_ANGLE.Pitch=asin（-2*q1*q3+2*q0*q2）

Q_ANGLE.Rool=atan2（2*q2*q3+2*q0*q1-2*q）

Q_ANGLE.Yaw=atan2（2*q1*q2+2*q0*q3，-2*q2*q2-2*q3*q3+1）

通過(guò)以上算法本文成功得出了飛行器的姿態(tài)角，在開(kāi)啟電機(jī)的情況下，角度誤差在+/-2°以?xún)?nèi)，滿(mǎn)足了控制要求。姿態(tài)計(jì)算效果如下圖所示，其中紅色和藍(lán)色是直接由加速度計(jì)算出的俯仰角和橫滾角，青色和黃色為姿態(tài)結(jié)算后的俯仰角和橫滾角。由圖中數(shù)據(jù)可看出，解算的姿態(tài)角不僅能即時(shí)的反應(yīng)角度變化切曲線(xiàn)平滑，說(shuō)明姿態(tài)解算算法有效。

3.2 控制設(shè)計(jì)

由于四軸飛行器獨(dú)特的機(jī)械結(jié)構(gòu)，即結(jié)構(gòu)上的對(duì)稱(chēng)設(shè)計(jì)，使得四軸在俯仰角的控制欲橫滾角的控制上有這近乎相同的控制特性，且兩者相對(duì)獨(dú)立。四軸飛行器的俯仰，橫滾，偏航，升降可以通過(guò)四個(gè)輸入量來(lái)控制。通過(guò)設(shè)定一個(gè)期望角度，調(diào)整電機(jī)轉(zhuǎn)速，使得測(cè)得的姿態(tài)角穩(wěn)定在期望角?？刂坡傻脑O(shè)計(jì)主要采用是閉環(huán)控制。以姿態(tài)角作為被控制量，采用經(jīng)典的PID控制算法。四軸飛行器系統(tǒng)是一個(gè)時(shí)變且非線(xiàn)性的系統(tǒng)，采用傳統(tǒng)PID算法的單一的反饋控制會(huì)使系統(tǒng)存在不同程度的超調(diào)和振蕩現(xiàn)象，無(wú)法得到理想的控制效果。文中將前饋控制引入到了四軸飛行器系統(tǒng)的控制中，有效地改善了系統(tǒng)的實(shí)時(shí)性，提高了系統(tǒng)的反應(yīng)速度；并且根據(jù)四軸飛行器系統(tǒng)的特點(diǎn)，對(duì)數(shù)字PID算法進(jìn)行了改進(jìn)，引入了微分先行環(huán)節(jié)，改善了系統(tǒng)的動(dòng)態(tài)特性；使得控制器能夠更好地適應(yīng)四軸飛行器系統(tǒng)的實(shí)際情況。在姿態(tài)角的控制中，本設(shè)計(jì)將控制器捕獲到的遙控器信號(hào)轉(zhuǎn)換為一個(gè)角度，作為期望角，與解算出來(lái)的測(cè)量角作差，得到偏差error。將error乘以一個(gè)比例系數(shù)kp。在只有kp作用下，系統(tǒng)會(huì)有靜差所以考慮利用積分ki控制消除穩(wěn)態(tài)誤差。但積分控制會(huì)降低系統(tǒng)的動(dòng)態(tài)性能，甚至造成閉環(huán)系統(tǒng)不穩(wěn)定，因此要對(duì)積分進(jìn)行限幅，防止積分過(guò)大。對(duì)于微分，如果采用傳統(tǒng)的D方法，在人為操縱四軸時(shí)會(huì)產(chǎn)生輸入的設(shè)定值變化頻繁且幅度較大，從而造成系統(tǒng)的振蕩。對(duì)人為控制十分不利，為了解決設(shè)定值的頻繁變化給系統(tǒng)帶來(lái)的不良影響，本文在姿態(tài)角控制上引入了微分先行PID算法，其特點(diǎn)是只對(duì)輸出量進(jìn)行微分，即只對(duì)陀螺儀角速度測(cè)量值進(jìn)行微分，而不對(duì)姿態(tài)角的設(shè)定值進(jìn)行微分。這樣，在設(shè)定值發(fā)生變化時(shí)，輸出量并不會(huì)改變，而被控量的變化相對(duì)是比較緩和的，這就很好地避免了設(shè)定值的頻繁變化給系統(tǒng)造成的振蕩，明顯地改善了系統(tǒng)的動(dòng)態(tài)性能?？刂浦芷诙? ms，姿態(tài)控制系統(tǒng)示意圖如下：

圖4 姿態(tài)控制系統(tǒng)示意圖

通過(guò)前面一章的介紹我們已經(jīng)的達(dá)到了俯仰，橫滾，航向3個(gè)控制量，然后將它們分別輸入3個(gè)獨(dú)立的如上圖所示的PID控制器，我們可以得到3個(gè)PID 輸出：pid_roll，pid_pitch，pid_yaw 將這 3 個(gè)輸出量做簡(jiǎn)單的線(xiàn)性運(yùn)算輸出給電機(jī)。部分代碼如下：

3.3 PID參數(shù)調(diào)節(jié)

1）確定比例增益P

確定比例增益P時(shí)，首先去掉PID的積分項(xiàng)和微分項(xiàng)，一般是令Ti=0、Td=0（具體見(jiàn)PID的參數(shù)設(shè)定說(shuō)明），使PID為純比例調(diào)節(jié)。輸入設(shè)定為系統(tǒng)允許的最大值的60%～70%，由0逐漸加大比例增益P，直至系統(tǒng)出現(xiàn)振蕩；再反過(guò)來(lái)，從此時(shí)的比例增益P逐漸減小，直至系統(tǒng)振蕩消失，記錄此時(shí)的比例增益P，設(shè)定PID的比例增益P為當(dāng)前值的60%～70%。比例增益P調(diào)試完成。

2）確定積分時(shí)間常數(shù)Ti

比例增益P確定后，設(shè)定一個(gè)較大的積分時(shí)間常數(shù)Ti的初值，然后逐漸減小Ti，直至系統(tǒng)出現(xiàn)振蕩，之后在反過(guò)來(lái)，逐漸加大Ti，直至系統(tǒng)振蕩消失。記錄此時(shí)的Ti，設(shè)定PID的積分時(shí)間常數(shù)Ti為當(dāng)前值的150%～180%。積分時(shí)間常數(shù)Ti調(diào)試完成。

3）確定積分時(shí)間常數(shù)Td

積分時(shí)間常數(shù)Td一般不用設(shè)定，為0即可。若要設(shè)定，與確定 P和Ti的方法相同，取不振蕩時(shí)的30%。

4）系統(tǒng)空載、帶載聯(lián)調(diào)，再對(duì)PID參數(shù)進(jìn)行微調(diào)，直至滿(mǎn)足要求。

最終調(diào)試好的標(biāo)準(zhǔn)應(yīng)該是，PID輸出曲線(xiàn)在有一個(gè)階躍響應(yīng)來(lái)是，響應(yīng)一大一小兩個(gè)波，小波是大波的四分之一。

3.4 調(diào)試與實(shí)驗(yàn)

在完成控制器底層的硬件驅(qū)動(dòng)后，開(kāi)始姿態(tài)角算法的調(diào)試。利用藍(lán)牙模塊將解算出的姿態(tài)角數(shù)據(jù)發(fā)回上位機(jī)，搖動(dòng)四軸機(jī)體，觀察上位機(jī)數(shù)據(jù)曲線(xiàn)與姿態(tài)演示立方塊。

圖中①和②為直接由加速度計(jì)算出的俯仰與橫滾角，③和④為姿態(tài)解算后的姿態(tài)角，可明顯看出直接由加速度計(jì)算出的姿態(tài)角噪聲大，不平滑，不能真實(shí)反映姿態(tài)角的變化。而由四元數(shù)算法解算后的姿態(tài)角反應(yīng)快，噪聲小，足以滿(mǎn)足控制要求。光觀察曲線(xiàn)并不直觀，還可以觀察上位機(jī)中的立方塊，當(dāng)小立方塊的姿態(tài)與四軸機(jī)體能夠保持一致變化時(shí)說(shuō)明姿態(tài)解算良好。通過(guò)以上手段可知姿態(tài)角解算滿(mǎn)足需求。

圖6 用于演示姿態(tài)的上位機(jī)立方塊

4 結(jié) 論

當(dāng)前面的各項(xiàng)調(diào)試都完成了之后，就準(zhǔn)備要進(jìn)行試飛實(shí)驗(yàn)。將4種飛行器放在水平地面上，開(kāi)始啟動(dòng)姿態(tài)初始化程序，聽(tīng)到電調(diào)提示音后，緩慢增加油門(mén)，螺旋槳轉(zhuǎn)速上升，將飛機(jī)拉離地面。由實(shí)際情況可看出當(dāng)姿態(tài)發(fā)生傾斜時(shí)，姿態(tài)解算及PID控制能夠及時(shí)調(diào)整電機(jī)轉(zhuǎn)速，穩(wěn)定飛行姿態(tài)。飛行器還能根據(jù)遙控指令的變化完成相應(yīng)的動(dòng)作。本此設(shè)計(jì)的四軸飛行器可實(shí)現(xiàn)垂直升降的要求，能保持姿態(tài)的穩(wěn)定，機(jī)體晃動(dòng)小，在微風(fēng)的干擾下能夠自動(dòng)調(diào)整姿態(tài)，確保平穩(wěn)飛行，且系統(tǒng)響應(yīng)快，續(xù)航時(shí)間大概在8分鐘。因此本次的設(shè)計(jì)是有效的。

[1]全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽組委會(huì).全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽獲獎(jiǎng)作品精選[M].北京：北京理工大學(xué)出版社，2013.

[2]李俊.四軸飛行器的動(dòng)力學(xué)模型及PID控制[J].遼寧工程技術(shù)大學(xué)學(xué)報(bào)：自然科學(xué)版，2012，31（1）：114-117.

[3]岳基?。⑿⌒退男頍o(wú)人機(jī)研究進(jìn)展及關(guān)鍵技術(shù)淺析[J].電光與控制，20l0，17（10）：46-52.

[4]劉峰，呂強(qiáng)，等．四軸飛行器姿態(tài)控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，2011，19（3）:583-585.

[5]張東偉，張曉斌，鄭先成.基于MP0566的飛機(jī)電源控制器系統(tǒng)設(shè)計(jì)[J].機(jī)械與電子，2010（6）:84-86.

[6]劉杰.四軸飛行器研究與設(shè)計(jì)[D].南京郵電大學(xué)，2013.

[7]解偉，張曉斌，張朋松，等.飛機(jī)電源控制器PCU測(cè)試儀的設(shè)計(jì)與實(shí)現(xiàn)[J].計(jì)算機(jī)測(cè)量與控制，2010，18（9）2207-2209.

[8]陸偉男，蔡啟仲，李剛，等.基于四軸飛行器的雙閉環(huán)PID控制[J].科學(xué)技術(shù)與工程，2014，14（33）：127-133.

[9]徐華中，余飛，何家俊，等.卡爾曼濾波在四軸飛行器導(dǎo)航中的應(yīng)用[J].武漢理工大學(xué)學(xué)報(bào)，2012，34（3）:265-267.

[10]關(guān)萍萍，翟正軍，姜紅梅，等.基于LabWindows/CVI測(cè)控系統(tǒng)通用報(bào)表的設(shè)計(jì)與實(shí)現(xiàn)[J].計(jì)算機(jī)工程與設(shè)計(jì)，2010，31（1）:203-205.

[11]孫驊.基于GPS和AHRS的四軸飛行器懸停算法研究[J].機(jī)械科學(xué)與技術(shù)，2013，32（4）：481-487.

[12]王楠楠.不確定非線(xiàn)性系統(tǒng)的輸出反饋控制[D].曲阜:曲阜師范大學(xué)，2010.

[13]王東來(lái)，呂強(qiáng)，劉峰，等.小型四軸飛行器動(dòng)力學(xué)參數(shù)測(cè)定方法設(shè)計(jì) [J].科技導(dǎo)報(bào)，2011，29（36）：42-45.

[14]成怡，金海林，修春波，等.四軸飛行器組合導(dǎo)航非線(xiàn)性濾波算法[J].計(jì)算機(jī)應(yīng)用，2014（A01）:341-344.

[15]龐慶霈.四旋翼飛行器設(shè)計(jì)與穩(wěn)定控制研究[D].合肥:中國(guó)科學(xué)技術(shù)大學(xué)，2011.

Four-rotor aircraft platform based on the STM32

YI Fan，DENG Fang，ZHANG Pei-jian，TENG Xuan-xuan
（School of Electric and Electronic Engineering ，Wuhan Polytechnic University，Wuhan 430040，China）

This design ，using a STM32F407 as the control core，uses a four-rotor aircraft as the carrier.Hardware consists of flight control circuit，power management，communication module，power system，aircraft frame，cloud deck.In algorithmic concept，the design uses the concise stable quaternion and complementary filter as a gesture decoding algorithm，taking PID as a controller to realize flight adjustments and other functions.The designer possesses qualities of flexible lightsome，ductility，good adaptability.

quad rotor； STM32； strap down inertial navigation； quaternion pose calculation

TN79

：A

：1674－6236（2017）14-0179-04

2016-06-07稿件編號(hào)：201606057

易 凡（1995—），男，湖北武漢人。研究方向：信號(hào)與信息處理、計(jì)算機(jī)測(cè)控。