基于STM32的多傳感器四旋翼姿態(tài)控制系統(tǒng)設(shè)計*

任劍秋，鐘小勇，張小紅

(江西理工大學(xué) 理學(xué)院，江西 贛州 341000)

0 引言

四旋翼是一種典型的多輸入輸出、非線性、強耦合的欠驅(qū)動系統(tǒng)[1]，控制系統(tǒng)復(fù)雜，但其結(jié)構(gòu)緊湊、靈活性和機動性好[2]，在軍事和民用領(lǐng)域都有廣泛的應(yīng)用前景。在四旋翼控制系統(tǒng)中，飛行的姿態(tài)控制尤為重要，直接影響無人機的控制精度和飛行效果[3]。四旋翼機載姿態(tài)傳感器受到環(huán)境如固有的振動、噪聲、磁場等客觀因素影響以及傳感器自身工藝的限制，可能導(dǎo)致傳感器測量的姿態(tài)信息與實際姿態(tài)有較大偏差[4-5]，要實現(xiàn)穩(wěn)定飛行，并非易事。為此，本文結(jié)合模塊化設(shè)計的思想，采用多傳感器數(shù)據(jù)融合，設(shè)計一種基于STM32的多傳感器四旋翼姿態(tài)控制系統(tǒng)。

1 四旋翼結(jié)構(gòu)及飛行原理

1.1 四旋翼無人機結(jié)構(gòu)

根據(jù)飛行器定義的機頭方向不同，無人機可分為十字型和X型兩種模式[6]。十字型布于鋼絲繩周圍的無人機的機頭方向指向某個旋翼，而X型無人機的機頭方向指向兩個旋翼中線?？紤]到今后加裝攝像頭等傳感器，為了不遮擋視線，本文采用X型無人機模式。X型無人機4個電機獨立分布于機臂四個頂點，如圖1所示，安裝在電機上的四個螺旋槳為無人機提供升力。四個旋翼按照旋轉(zhuǎn)方向不同，有正反槳之分，同一對角線上的旋翼旋轉(zhuǎn)方向相同，其中電機1、3逆時針旋轉(zhuǎn)為反槳，電機2、4順時針旋轉(zhuǎn)為正槳。正反槳的設(shè)計使飛行器的陀螺效應(yīng)和空氣扭矩效應(yīng)相互抵消，從而保證四旋翼的平穩(wěn)飛行[7]。

圖1 四旋翼結(jié)構(gòu)模型

1.2 四旋翼無人機運動姿態(tài)

四旋翼無人機通過四個電機轉(zhuǎn)速的改變來控制四個旋翼產(chǎn)生的升力，進而控制無人機的運動。無人機運動可以分解為俯仰、橫滾、偏航、垂直四種基本的運動。通過對無人機這四個基本運動的控制，可以實現(xiàn)無人機在地理坐標(biāo)系中升降、前后、左右、偏航等運動。同時提高(降低)電機2、3的轉(zhuǎn)速，降低(提高)電機1、4的轉(zhuǎn)速，使旋翼產(chǎn)生的轉(zhuǎn)矩不平衡，可以實現(xiàn)飛行器的俯(仰)姿態(tài)，進而控制無人機前后運動。同理，同時提高(降低)電機1、2的轉(zhuǎn)速，降低(提高)電機3、4的轉(zhuǎn)速，可以實現(xiàn)飛行器的橫滾姿態(tài)，進而控制無人機左右運動。同時提高(降低)電機1、3的轉(zhuǎn)速，降低(提高)電機2、4的轉(zhuǎn)速，使旋翼產(chǎn)生的反扭矩不平衡，可以實現(xiàn)飛行器的偏航姿態(tài)，進而控制無人機順(逆)時針的偏航運動。同時提高(降低)電機1、2、3、4的轉(zhuǎn)速，可以控制無人機上升(下降)運動，當(dāng)旋翼產(chǎn)生的總升力剛好抵消無人機重力時，無人機完成懸停動作。

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

四旋翼無人機控制系統(tǒng)包括主控制器，姿態(tài)及位置多種傳感器，驅(qū)動、遙控、通信、電源等模塊。主控制器通過采集傳感器的實時數(shù)據(jù)信息，進行姿態(tài)解算，得到無人機的姿態(tài)及位置信息，并根據(jù)遙控器指令，結(jié)合相關(guān)算法控制無人機的姿態(tài)。控制系統(tǒng)硬件如圖2所示。

圖2 控制系統(tǒng)硬件示意圖

2.1 主控芯片

本設(shè)計采用的ARM Cortex M4內(nèi)核的32位STM32-F407ZET6芯片作為主控芯片。該系列芯片具有168 MHz最大頻率、PWM高速定時器、全雙工I2C、高速USART、高速SPI等性能特點，能夠滿足四旋翼無人機數(shù)據(jù)處理及控制要求。

2.2 驅(qū)動模塊

四旋翼無人機對電機的轉(zhuǎn)速要求較高，控制系統(tǒng)采用無刷直流電機。主控制器經(jīng)過電子調(diào)速器驅(qū)動直流電機，通過控制PWM波占空比調(diào)節(jié)電機轉(zhuǎn)速。電機選用朗宇X2212-13，980 kV無刷直流電機，其響應(yīng)速度快、調(diào)速范圍大，可靠性高。電調(diào)選用好盈天行者30 A直流電調(diào)，能夠持續(xù)穩(wěn)定提供30 A輸出電流，可對無刷電機進行快速調(diào)節(jié)。針對以上選用的電機和電調(diào)，配套使用直徑為10 mm、螺距為4.5 mm的正反槳為無人機提供升力。

2.3 通信模塊

通信模塊選用ATK-WLDBG無線調(diào)試器，調(diào)試器分為地面端與飛控端，兩者間采用2.4 G無線通信。在地面端以921 600波特率向上位機發(fā)送數(shù)據(jù)，實現(xiàn)無線仿真調(diào)試及數(shù)據(jù)傳遞；在飛控端通過UART與主控制器相連。另外，在飛控端備用了一個4P的SWD接口，支持MDK編輯器，無線程序燒錄。

2.4 遙控模塊

本設(shè)計選用天地飛7通道遙控器WFT07。遙控器發(fā)送用戶指令，通過2.4 G無線技術(shù)發(fā)射各通道PWM信號。機載接收器接收到信號后，發(fā)送到主控制器定時器通道。主控系統(tǒng)利用定時器的輸入捕獲功能得到遙控器的期望指令數(shù)據(jù)。

2.5 姿態(tài)傳感器

姿態(tài)傳感器采用MPU6050采集無人機的角速度與加速度原始信息。MPU6050集成3軸陀螺儀，3軸MEMS加速度計，其測量范圍可根據(jù)器件寄存器的數(shù)據(jù)位進行設(shè)置。在本控制系統(tǒng)中，加速度量程設(shè)為±16 g，角速度量程設(shè)為±2 000 °/s。為了確定偏航角及誤差修正、補償，MPU6050通過I2C接口(數(shù)據(jù)線SDA_ES、時鐘線SCL_ES)與磁傳感器HMC5883L相連，組成9軸傳感器，具體電路如圖3所示。主控芯片同樣通過I2C接口(數(shù)據(jù)線SDA、時鐘線SCL)與MPU6050相連接，并以100 Hz頻率采集姿態(tài)傳感器數(shù)據(jù)。

圖3 姿態(tài)傳感器應(yīng)用電路

2.6 位置傳感器

選用壓力傳感器SPL06-001來測量四旋翼無人機的飛行高度，測壓范圍為300～1 200 hPa，工作溫度范圍為-40 ℃～85 ℃，最高采樣率可達300 Hz，分辨力為5 cm。相對傳統(tǒng)的傳感器而言，其氣壓精度和靈敏度更高。SPL06-001芯片通過SPI總線與主控芯片相連，具體電路如圖4所示。

圖4 壓力傳感器應(yīng)用電路

水平位置傳感器采用SE-100 GPS傳感器采集無人機室外的經(jīng)緯度信息，其定位精度最高可以達到0.5 m，測速精度為0.1 m/s，更新頻率為18 Hz，主控芯片通過UART以9 600波特率采集傳感器數(shù)據(jù)。

2.7 電源模塊

電源選用格式4 000 mA、25C、3S鋰電池為無人機供電。鋰電池除了為電調(diào)供電，同時作為輸入電壓經(jīng)MP2359芯片穩(wěn)壓至5 V，再經(jīng)AMS1117降壓至3.3 V，供給主控芯片、各個傳感器及模塊電源。相對于由電調(diào)供應(yīng)5 V電壓方式，此供電穩(wěn)壓效果更好，具體電路如圖5所示。為了調(diào)試、下載方便，5 V電源也可采用USB供電。另外，為了避免電量引起事故，控制系統(tǒng)還設(shè)計了電壓檢測及報警電路。當(dāng)鋰電池電壓低于某個數(shù)值時，控制系統(tǒng)報警示意并啟動相關(guān)的安全措施。

圖5 電源及報警電路

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

本文采用C語言編程，確保通電后各個功能模塊可以正常工作，并可以根據(jù)遙控指令實現(xiàn)無人機的穩(wěn)定飛行控制。系統(tǒng)上電后完成各模塊的初始化，定時器設(shè)置5 ms的控制周期，每次中斷任務(wù)請求讀取傳感器模塊的數(shù)據(jù)，經(jīng)過處理得到無人機的實時姿態(tài)數(shù)據(jù)，同時讀取遙控器的指令，將其作為期望數(shù)據(jù)傳入PID控制器，輸出4路PWM信號控制四旋翼4個電機的轉(zhuǎn)速，從而實現(xiàn)對無人機的飛行控制。軟件控制流程如圖6所示。

圖6 軟件控制流程圖

3.1 姿態(tài)解算

3.1.1 歐拉角與四元數(shù)

根據(jù)姿態(tài)傳感器測量的角速度值、加速度值解算出無人機的實際姿態(tài)角，將其作為反饋量用于控制無人機的飛行。本文將傳感器得到的原始數(shù)據(jù)通過四元數(shù)方程解算姿態(tài)角度，避免使用歐拉角可能出現(xiàn)的萬向節(jié)死鎖，同時還能避免大量的三角函數(shù)運算，這樣可減少計算量。

本文采用的地理坐標(biāo)系(n系)為北西天；載體坐標(biāo)系(b系)為前左上，即機體x軸指向前方，y軸指向左方，z軸指向上方，參見圖1。在研究物體轉(zhuǎn)動和位置的變化中，常用俯仰角θ、橫滾角φ、航向角Ψ 來表示確定向量的轉(zhuǎn)動位置。無人機各軸的角度變化用歐拉角表示，需要依次繞三個軸進行旋轉(zhuǎn)才能求出復(fù)合后的旋轉(zhuǎn)矩陣，本文采用z、y、x的旋轉(zhuǎn)順序。用歐拉角來表示無人機從載體坐標(biāo)系b旋轉(zhuǎn)到地理坐標(biāo)系n的旋轉(zhuǎn)矩陣公式為[8]：

式中c、s分別表示cos和sin函數(shù)。

3.1.2 互補濾波

對于MPU6050來說，陀螺儀在b系中積分得到的角度不受加速度的影響，但是隨著時間的增加，積分漂移和溫度漂移帶來的誤差會比較大。加速度計對無人機的加速度比較敏感，但取瞬時值計算傾角誤差比較大，應(yīng)合理應(yīng)用補償算法校正誤差[10]。本文使用加速度計數(shù)據(jù)，通過互補濾波法對陀螺儀數(shù)據(jù)進行處理，以獲得較高精度的校正結(jié)果。

互補濾波主要以陀螺儀測得數(shù)據(jù)為主，通過加速度計測量值來彌補誤差。互補濾波法姿態(tài)解算的具體步驟如下：

(1)靜止?fàn)顟B(tài)下加速度計測得的地理系加速度值應(yīng)為[0 0 g]T，歸一化處理后，轉(zhuǎn)換到載體系，理想重力分量應(yīng)為：

(2)將加速度計測量的加速度數(shù)據(jù)歸一化。

(3)加速度計歸一化后的值與重力向量叉乘，得到三軸的誤差向量。

(4)得到三軸的誤差向量后，分別使用PI控制器，對陀螺儀測量的角速度進行補償。

其中，eInt為積分項，ts為傳感器采樣周期，kp為比例系數(shù)，ki為積分系數(shù)，ωx、ωy、ωz為陀螺儀測量的三軸角速度，為補償后的三軸角速度。

(5)用補償后的角速度值，使用一階畢卡法更新四元數(shù)，四元數(shù)更新表達式如下：

在初始時刻給定一個四元數(shù)初值，即可通過陀螺儀測得的角速度不斷更新四元數(shù)。

(6)將更新后的四元數(shù)進行歸一化處理，按照式(3)計算實際的姿態(tài)角度。

3.2 串級PID姿態(tài)控制

PID算法在控制工程實踐中應(yīng)用廣泛，并且不需要精確的物理模型[11-12]。本文選用串級PID控制策略，以提高無人機飛行過程中的干擾能力和適應(yīng)性。串級PID分為內(nèi)、外兩環(huán)，外環(huán)為角度PID環(huán)，內(nèi)環(huán)為角速度PID環(huán)。外環(huán)輸入為遙控器給定的期望角度與姿態(tài)解算的實際角度偏差，通過外環(huán)PID后，將輸出的期望角速度與傳感器反饋的實際角速度偏差作為內(nèi)環(huán)輸入，最終轉(zhuǎn)換為PWM波信號控制電機轉(zhuǎn)速。姿態(tài)控制系統(tǒng)流程圖如圖7所示。

圖7 姿態(tài)串級PID控制流程圖

4 實驗結(jié)果與分析

本文設(shè)計的X型四旋翼無人機實物圖如圖8所示。無人機姿態(tài)控制中串級PID參數(shù)的合適與否直接決定了無人機是否能夠穩(wěn)定飛行。對無人機的俯仰、橫滾以及航向三個姿態(tài)通道分別進行PID參數(shù)的整定，三個通道整定的PID參數(shù)如表1所示。

圖8 四旋翼無人機實物圖

表1 三個姿態(tài)通道的PID參數(shù)

為了檢驗控制系統(tǒng)的效果，遙控器分別給定橫滾和俯仰兩個通道的期望角度值，主控芯片通過通信模塊將姿態(tài)數(shù)據(jù)發(fā)送到上位機，并在上位機上進行波形顯示。圖9和圖10分別是橫滾角和俯仰角實際角度跟隨期望角度的變化情況。從圖9和圖10可以看出，無人機實際角度可以快速、穩(wěn)定地追蹤到期望角度，并且在沒有給定期望角度即默認為0°時，無人機姿態(tài)的實際角度可以穩(wěn)定在0°。實驗驗證了四旋翼無人機姿態(tài)控制系統(tǒng)的有效性以及無人機飛行的穩(wěn)定性。

圖9 橫滾角度變化

圖10 俯仰角度變化

5 結(jié)論

四旋翼無人機在飛行過程中姿態(tài)傳感器容易受到復(fù)雜氣流、磁場干擾、槳葉震動等因素影響，姿態(tài)控制的不精準(zhǔn)會造成無人機機體抖動、漂移甚至失控等問題，為此本文設(shè)計了一種基于STM32的多傳感器四旋翼無人機控制系統(tǒng)。實際飛行試驗表明，該系統(tǒng)可快速靈活地控制四旋翼姿態(tài)，并實現(xiàn)四旋翼穩(wěn)定飛行。同時，也為集成其他傳感器模塊，實現(xiàn)導(dǎo)航、循跡、跟蹤等功能奠定了基礎(chǔ)。