基于STM32的多傳感器四旋翼姿態控制系統設計*

任劍秋，鐘小勇，張小紅

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

0 引言

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

1 四旋翼結構及飛行原理

1.1 四旋翼無人機結構

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

圖1 四旋翼結構模型

1.2 四旋翼無人機運動姿態

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

2 系統硬件設計

四旋翼無人機控制系統包括主控制器，姿態及位置多種傳感器，驅動、遙控、通信、電源等模塊。主控制器通過采集傳感器的實時數據信息，進行姿態解算，得到無人機的姿態及位置信息，并根據遙控器指令，結合相關算法控制無人機的姿態。控制系統硬件如圖2所示。

圖2 控制系統硬件示意圖

2.1 主控芯片

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

2.2 驅動模塊

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

2.3 通信模塊

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

2.4 遙控模塊

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

2.5 姿態傳感器

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

圖3 姿態傳感器應用電路

2.6 位置傳感器

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

圖4 壓力傳感器應用電路

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

2.7 電源模塊

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

圖5 電源及報警電路

3 系統軟件設計

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

圖6 軟件控制流程圖

3.1 姿態解算

3.1.1 歐拉角與四元數

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

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

式中c、s分別表示cos和sin函數。

3.1.2 互補濾波

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

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

(1)靜止狀態下加速度計測得的地理系加速度值應為[0 0 g]T，歸一化處理后，轉換到載體系，理想重力分量應為：

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

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

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

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

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

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

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

3.2 串級PID姿態控制

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

圖7 姿態串級PID控制流程圖

4 實驗結果與分析

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

圖8 四旋翼無人機實物圖

表1 三個姿態通道的PID參數

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

圖9 橫滾角度變化

圖10 俯仰角度變化

5 結論

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