基于STM32的微型四旋翼飛行器的設計與研究

李亞杰 葛 宇 張 曄

(河北建筑工程學院，河北 張家口 075000)

0 引 言

四旋翼無人機是一種新興的無人機，多年前被軍方科學家放棄的它，近幾年大放異彩，快速發展起來，它可以利用Wi-Fi遙控和自帶硬件及軟件算法進行飛行控制的一種新型飛行器，最開始的時候是軍方為了某些軍事活動而專門研發的.相比于常規的固定翼或是單翼的直升機，四旋翼無人機具備靈活的垂直起降和非常穩定的自主懸停功能，低空飛行的同時可以靈便迅速地變換機身方向，非常適合在復雜的環境中執行飛行任務[1].

1 四軸飛行器的硬件電路設計

微型四軸飛行器的硬件電路由微控制器模塊、無線通訊模塊、傳感器模塊、電源模塊、動力系統模塊組成.

圖1-1 主控MCU

1.1 微控制器模塊

主控MCU型號為STM32F411CEU6,資源豐富，有一百多個引腳，六組IO口均具備復用功能.以上電路參考其附帶的數據手冊中的經典電路.

圖1-2 動力系統模塊

1.2 動力系統模塊

動力系統使用高速716空心杯電機，最高速電機轉速高達每分鐘四萬轉，能提供強大升力，而空心杯電機本身結構簡單，這也使其驅動方式簡單容易，使用NMOS管如圖所示接法[2]，在3v的電勢差下，門級間電阻非常小，驅動電流最高可達3A，完全可以使空心杯電機運行在正常狀態[3].

圖1-3 傳感器模塊電路

1.3 傳感器數據采集模塊

MPU9250為九軸傳感器芯片，九軸分別為3軸加速度、3軸陀螺儀和3軸磁力計，自帶數字運動處理器(即DMP).

圖1-4 無線通信芯片

1.4 無線通訊模塊

NRF51822功能強大，是一款具備藍牙功能和2.4GHz通訊的無線通訊芯片.他使用方式靈活，支持SPI，串口通訊等通信方式，可以很容易的實現芯片間的級聯協作.如右邊的RFX2401C，它可以顯著提高NRF51822的性能，增加其傳輸距離和信號強度，并且RFX2401C帶有IEEE 802.15.4/Zigbee協議，這意味著它可以輕易接入ZigBee物聯網，從而發揮更大的作用[4].

2 數據融合與姿態解算

2.1 姿態解算簡介

姿態解算的意思是處理器獲得四軸sensor數據，實時計算四軸飛行器機身坐標系與地理坐標系之間的夾角，稱之為姿態角，處理器根據這些姿態角的大小信息，可以推算出當前飛機在空中的姿態，根據擾動干擾因素噪聲以及控制需要計算4個電機的輸出量，使飛行器保持平衡自穩或者保持機身傾斜往設定方向飛行[5-6].

2.2 姿態解算

硬件解算使用DMP處理過數據后，直接讀取數據，主控的MCU只起調配控制作用，不進行數據處理.軟件解算就是動用主控MCU的資源對數據進行處理，MPU9250只起測量數據的作用，得到數據后，等待MCU的讀取數據指令，把數據傳給MCU通過公式運算，和轉換過程最終得到想要的數據.兩種方法各有優勢，下面對這兩種方法的數據處理過程分別闡述，以對比的方式選擇最為高效可靠的姿態解算方式.軟件姿態解算以下步驟為以四元數形式采用互補濾波算法進行解算的過程：

(1)初始化四元數初始化公式：

(2)測量讀取加速度和角速度(初始數據)并歸一化.

得到加速度計的數據和坨摞儀的數據、加速度計數據記錄為、坨摞儀數據分別記錄為gx,gy,gz.

單位化加速度計的三維向量留用：

Norm=1/sqrt(ACCx*ACCx+ACCy*ACCy+ACCz*ACCz)

ACCx=ACCx*Norm

ACCy=ACCy*Norm

ACCz=ZCCz*Norm

(2-2)

(3)提取四元數的等效余弦矩陣中的重力分量.

重力加速度記為Vx、Vy、Vz:

Vx=2*(q1*q3-q0*q2)

Vy=2*(q0*q1-q2*q3)

Vz=q0*q0-q1*q1-q2*q2+q3*q3

(2-3)

(4)向量叉積得出姿態誤差.

將加速度計與重力加速度做差值記作Α、Β、Γ，通過計算得出這三個值:

A=ACCy*Vz-ACCz*Vy

B=ACCz*Vx-ACCx*Vz

Γ=ACCx*Vy-ACCy*Vx

(2-4)

(5)對誤差進行積分.

誤差累計值記為Exint，Eyint，Ezint：

Exint=Exint+Ez*KI

Eyint=Eyint+Ez*KI

Ezint=Ezint+Ez*KI

(2-5)

(6)互補濾波，姿態誤差與角速度做和，修正角速度積分漂移(誤差，偏差)：

gx=gx+KP*A+Exint

gy=gy+KP*B+Eyint

gz=gz+KP*Γ+Ezint

(2-6)

(7)一階龍格庫塔法或一階畢卡法更新四元數.

Q0=q0+(-q1*gx-q2*gy-q3*gz)*t/2

Q1=q1+(q0*gx-q3*gy+q2*gz)*t/2

Q2=q2+(q0*gy-q1*gz+q3*gx)*t/2

Q3=q3+(q0*gz+q1*gy-q2*gx)*t/2

(2-7)

(8)四元數歸一化.

Norm=1/sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3)

Q0=Q0*Norm

Q1=Q1*Norm

Q2=Q2*Norm

Q3=Q3*Norm

(2-8)

(9)四元數轉歐拉角.

使用下面公式轉換即可：

3 四軸飛行器的PID控制

其過程和情況如下圖，將四軸飛行器固定在高強度精密碳纖維管材料上，搭起支架，將其懸置于半空中，具體操作如下圖：

圖3-1 調試硬件操作圖

第二步遙控器連接上位機

使用通用串行總線將遙控器連接到上位機，如圖設置數據，寫入單片機，如圖3-2所示.

圖3-2 PID初始設置示圖

第三步，調內環pitch和roll的P設置為手動控制模式.

按照第三步操作后，設置內圈ROL率和PIT率的P值相同，然后將油門推到50左右，觀察并記錄現象：

P=3500時，四軸機身輕微晃動，用手拉動能體會到很大的反抗力.這是其實已經很理想了，四軸表現狀態也很正常，不過仍可適當減少使其更加趨近于理想值.

P=3200時，四軸不再晃動，用手去干擾能感覺到適中大小的回復力.可以認為當前的P值就是理想值了，擺動問題可以通過提高D抑制.

第四步,基本思路同第三步，但需設置P值為3200(因為P已經調好)，測試不同的D值對應的現象：

令D等于50，受到干擾，基本不會晃動了，說明D值接近理想值了.

第五步，內環調好了，下面開始調外環.外環PID決定了控制四軸姿態響應快慢，測試不同的P值時，觀察四軸反應快慢.按照調內環的方法，測試理想P值，現象如下：

P=10時，使油門到最大值，雖然四軸最終達到了預設姿態，但過程緩慢，反應遲鈍，表明P遠小于理想值.

P=100時，同樣使油門到最大值，四軸瞬間傾斜，反應劇烈，一下子就達到預設姿態，而且感覺沖擊感很強，慣性很大，說明調整姿態的力過大，這說明P值太大了.

P=50時，重復操作，四軸較快的達到設定角度，速度適中，機身反應緩和，動作感覺干凈利索，這說明P值是理想值了.

第六步，調內環航向角的PID調節內環航向角的PID前需將第一步去掉的宏定義代碼重新補充回來，并且使用keil編譯連接成功后用swd下載調試器下載到STM32中.之后設置航向角外環P初始值為十，內環P設定為一適宜值，慢慢推油門讓四軸起飛至水平平衡，推油門搖桿時晃動，以保持機頭方向不改變.四軸水平穩定后，推油門至稍大，然后用手稍微轉動四軸，感受四軸的回復力，憑聲音粗略判別電機轉速變化.下面是當P值不同時對應的現象為：

當P=10時，四軸回力微弱，人為干擾后四軸搖晃明顯，沒有電機喧囂聲，說明沒有產生電機差速，更不會產生修正的回復力，這說明P值太小了.

當P=100時，四軸回復力稍微變大，說明P值還是太小.

當P=1000時，四軸回復力很大，手動干擾后比較穩定.說明P值接近理想值了.

當P=5000時，手動干擾后電機停轉，電機聲音嘯叫，非常大，這說明P值已經太大了.

根據不斷試驗以逼近理想值最終選定當P值為1200時效果最佳，然后再稍微提高積分I，不必要提高地.內環的反饋航向角也是由陀螺儀的數據積分而得，使得D不僅不助于機體穩定，反而破壞機體穩定性，所以最好的選擇是不添加D.

第七步，調外環航向角的PID調節外環航向角的P和調節外環俯仰角/橫滾角的P方法是一樣的，主要目的是調節四軸的受控的響應速度和效果.設置外環偏航角的P一個適宜值加大油門大幅改變航向角，用手干擾感受四軸旋轉的力度.最終經試驗重復推敲選定P的值為100.

圖3-3 四軸PID參數

第八步手動試飛.將四軸放在戶外寬闊的水平地面上測試手動飛行.在無風條件下，如果四軸飛行時產生不正?；蝿诱f明內環俯仰角和橫滾角的P值高了，這時可以通過削弱P或提高D來抑制晃動.這是通常會出現兩種現象：一是航向角受外力干擾不能快速回正，二是飛行過程四軸在沒有轉向指令的情況下會慢慢自旋.前者需提高P值，后者需減小積分項的值.經過以上反復調試，最終確定四軸PID參數如右圖所示：

第九步，調定高的PID

按步驟調節定高PID，反復調試但達不到理想效果，四軸高度飄忽不定，忽高忽低.后來經在軟件上調試，發現使用氣壓計傳感器所獲得的氣壓數據有很大缺缺陷，不僅不準確，而且響應速度很慢，有滯后性，而且四軸飛行過程由于流體流速大的地方壓強小，電機掀起的風以及自然風都會對氣壓計所測數據產生嚴重影響.因此最好的解決辦法就是更換模塊，改用GPS測海拔高度可以解決這個問題，或者采用高成本高精度的具有濾波效果的氣壓計，考慮到資金與技術原因，均不易實現，只能根據D的抑制超調和干擾的作用，增加一點D值，勉強有一定效果.而D值也不能太大了，否則會將P的作用都抑制抵消了，起反作用.反復調試后，最后敲定PID的三個值的大小為220、5、580.

4 總 結

四軸飛行器的穩定飛行不只是原來單純看到的那樣只要四個電機平衡了就可以，實際上需要強大的數據融合算法來提供變量，PID控制算法進行精細控制，以此消除誤差，抵抗外界的不定期不定強度的干擾，這樣才能真正實現四軸飛行器的穩定飛行.