基于STM32四軸飛行平臺

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

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

基于STM32四軸飛行平臺

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

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

本設計以STM32F407為控制核心，四軸飛行器為載體。硬件上由飛控電路，電源管理，通信模塊，動力系統，機架，云臺伺服系統組成。算法上采用簡潔穩定的四元數加互補濾波作為姿態解算算法，PID作為控制器，實現飛行，云臺增穩等功能。具有靈活輕盈，延展性，適應性強好等特點。

四旋翼飛行器；STM32；捷聯式慣性導航；四元數姿態解算

四軸飛行器是一種利用4個旋翼作為飛行引擎來進行空中飛行的飛行器。進入20世紀以來，電子技術飛速發展四軸飛行器開始走向小型化，并融入了人工智能，使其發展趨于無人機，智能機器人。在實際應用方面，四軸飛行器可以在復雜、危險的環境下可以完成特定的飛行任務，也可以用于監控交通，環境等。本設計主要研究了四軸飛行器的姿態結算和飛行控制，并設計制作了一架四軸飛行器，對關鍵傳感器做了標定，并利用用matlab分析數據，設計算法，還進行了單通道平衡試驗調試，進行試飛實驗取得了一定的效果。

1 系統方案

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

圖1 系統總體框圖

2 系統硬件設計

針對前面提出的整體設計方案，本設計采取模塊化策略，將各個功能部分開來設計，最后組合起來。

表1 四軸飛行器硬件清單

3 系統軟件設計

軟件設計上由控制核心STM32F4讀取傳感器信息，解算姿態角，以姿態角為被控制量融合遙控信息后，輸出到4個電機及兩個舵機以完成四軸飛行控制和云臺的穩定補償。下圖是軟件流程：

圖2 系統軟件流程

3.1 四元素計算姿態角的實現

根據前面給出的姿態解算方程與四元數，即可得到姿態計算系統的計算原理如下圖。

圖3 姿態計算系統計算原理圖

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

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

然后利用向量的叉積，e→可視為誤差向量，這個叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體坐標系的糾正。用上面得到的結果校正陀螺儀：

此處k為一個常量系數。再利用二階畢卡法解四元數微分方程，更新四元數為下一次計算做準備。畢卡二階算法為：

其中Δθ為角增量

最后將四元數轉變為歐拉角：

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）

通過以上算法本文成功得出了飛行器的姿態角，在開啟電機的情況下，角度誤差在+/-2°以內，滿足了控制要求。姿態計算效果如下圖所示，其中紅色和藍色是直接由加速度計算出的俯仰角和橫滾角，青色和黃色為姿態結算后的俯仰角和橫滾角。由圖中數據可看出，解算的姿態角不僅能即時的反應角度變化切曲線平滑，說明姿態解算算法有效。

3.2 控制設計

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

圖4 姿態控制系統示意圖

通過前面一章的介紹我們已經的達到了俯仰，橫滾，航向3個控制量，然后將它們分別輸入3個獨立的如上圖所示的PID控制器，我們可以得到3個PID 輸出：pid_roll，pid_pitch，pid_yaw 將這 3 個輸出量做簡單的線性運算輸出給電機。部分代碼如下：

3.3 PID參數調節

1）確定比例增益P

確定比例增益P時，首先去掉PID的積分項和微分項，一般是令Ti=0、Td=0（具體見PID的參數設定說明），使PID為純比例調節。輸入設定為系統允許的最大值的60%～70%，由0逐漸加大比例增益P，直至系統出現振蕩；再反過來，從此時的比例增益P逐漸減小，直至系統振蕩消失，記錄此時的比例增益P，設定PID的比例增益P為當前值的60%～70%。比例增益P調試完成。

2）確定積分時間常數Ti

比例增益P確定后，設定一個較大的積分時間常數Ti的初值，然后逐漸減小Ti，直至系統出現振蕩，之后在反過來，逐漸加大Ti，直至系統振蕩消失。記錄此時的Ti，設定PID的積分時間常數Ti為當前值的150%～180%。積分時間常數Ti調試完成。

3）確定積分時間常數Td

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

4）系統空載、帶載聯調，再對PID參數進行微調，直至滿足要求。

最終調試好的標準應該是，PID輸出曲線在有一個階躍響應來是，響應一大一小兩個波，小波是大波的四分之一。

3.4 調試與實驗

在完成控制器底層的硬件驅動后，開始姿態角算法的調試。利用藍牙模塊將解算出的姿態角數據發回上位機，搖動四軸機體，觀察上位機數據曲線與姿態演示立方塊。

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

圖6 用于演示姿態的上位機立方塊

4 結 論

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

[1]全國大學生電子設計競賽組委會.全國大學生電子設計競賽獲獎作品精選[M].北京：北京理工大學出版社，2013.

[2]李俊.四軸飛行器的動力學模型及PID控制[J].遼寧工程技術大學學報：自然科學版，2012，31（1）：114-117.

[3]岳基?。⑿⌒退男頍o人機研究進展及關鍵技術淺析[J].電光與控制，20l0，17（10）：46-52.

[4]劉峰，呂強，等．四軸飛行器姿態控制系統設計[J].計算機測量與控制，2011，19（3）:583-585.

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

[6]劉杰.四軸飛行器研究與設計[D].南京郵電大學，2013.

[7]解偉，張曉斌，張朋松，等.飛機電源控制器PCU測試儀的設計與實現[J].計算機測量與控制，2010，18（9）2207-2209.

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

[9]徐華中，余飛，何家俊，等.卡爾曼濾波在四軸飛行器導航中的應用[J].武漢理工大學學報，2012，34（3）:265-267.

[10]關萍萍，翟正軍，姜紅梅，等.基于LabWindows/CVI測控系統通用報表的設計與實現[J].計算機工程與設計，2010，31（1）:203-205.

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

[12]王楠楠.不確定非線性系統的輸出反饋控制[D].曲阜:曲阜師范大學，2010.

[13]王東來，呂強，劉峰，等.小型四軸飛行器動力學參數測定方法設計 [J].科技導報，2011，29（36）：42-45.

[14]成怡，金海林，修春波，等.四軸飛行器組合導航非線性濾波算法[J].計算機應用，2014（A01）:341-344.

[15]龐慶霈.四旋翼飛行器設計與穩定控制研究[D].合肥:中國科學技術大學，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稿件編號：201606057

易 凡（1995—），男，湖北武漢人。研究方向：信號與信息處理、計算機測控。