基于單片機的四旋翼飛行器

韋凱 王鵬

摘要：近年來隨著科技技術的發展，無人機在各個領域的應用及其廣泛，而現在的無人機，已經趨向人工智能，趨向迷你化，多功能化。而四旋翼飛行器與其它飛行器相比，更具有易于操縱，穩定性高，應用性更強等優點。因此對于基于單片機的四旋翼飛行器的研究，將擴大無人機的工作領域，并且可以降低造價，對于我國無人機方面做出一些貢獻。

關鍵詞：四旋翼飛行器；STM32單片機；APM飛控系統

一、前言

該四旋翼飛行器由四個旋翼作為動力來源，產生升力，并且因為其為對稱結構，使其具有其它無人機沒有的功能和特性。本設計采用STM32單片機作為飛行器的控制器，內部由主控模塊、飛行模塊、光電感應板、無刷電機等部分組成，用PID控制姿態角以及濾波等對姿態角進行處理，測定后經PID輸出相應電機的PWM增減量，然后作用于陀螺儀，陀螺儀控制機身的飛行軌跡和姿態；通過對陀螺儀的校正反作用于機身實現定高及飛行等任務。

二、設計方案

電源及驅動模塊：對于電源模塊，因為四旋翼飛行器需要保持輕便，長久的使用，所以需要使用重量較輕，容量較大的鋰電池進行供電，考慮到芯片工作所需的電壓，所以我們采用將11.4V的鋰電池進行穩壓后的3V狀態進行工作。并且加入了600mA的保險絲，保護芯片的安全。對于驅動方面，我們采用無刷電機。

無刷電機具有使用壽命長，噪音低，成本較低等優點，因為其為自控試運行方式，所以不會產生震蕩和失步，更穩定。

三、系統工作原理

軟件設計上由單片機STM32讀取傳感器信息，解算姿態角，以姿態角為被控制量融合遙控信息后，輸出到四個無刷電機以及兩個舵機以完成四軸飛行控制和云臺的穩定補償。

四、姿態角計算

依據姿態解算方程和四元數，

PID的控制規律為下圖所示：

在此理論基礎上，即可通過矯正后的陀螺儀進行分析后得到姿態角。我們可設出加速度，加速度的向量，以及陀螺儀的數據和陀螺儀向量。（均在X，Y，Z坐標下進行）然后再帶入四元數形式轉換矩陣，再根據余弦矩陣和歐拉角的定義，將地理加速度的向量應用到陀螺儀上，機體對于陀螺儀是求導的關系，所以對陀螺儀進行校正即對機體進行校正，以此得到校正后的陀螺儀。再利用畢卡二階算法：

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

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

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

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

由此得到了飛行器的姿態角，并且可以準確反映飛行器的運行軌跡，驗證其姿態角的標準性。

五、部分程序

void IMUupdate（double gx，double gy，double gz，doubleax，double ay，double az）

{

static float q0 = 1.0f，q1 = 0.0f，q2 = 0.0f，q3 = 0.0f；

double delta_2=0；

const static doubleFACTOR = 0.002；

doublenorm=0.0f；

double vx，vy，vz；

double ex，ey，ez；

double q0q0 = q0*q0；

double q0q1 = q0*q1；

double q0q2 = q0*q2；

double q1q1 = q1*q1；

double q1q3 = q1*q3；

double q2q2 = q2*q2；

double q2q3 = q2*q3；

double q3q3 = q3*q3；

norm = sqrt（ax*ax）+sqrt（ay*ay）+sqrt（az*az）； // 測量標準化 把加速度的三維向量轉為單位向量。

ax = ax /norm；

ay = ay / norm；

az = az / norm；

vx = 2*（q1q3 - q0q2）； // 估計方向的重力 vy = 2*（q0q1 + q2q3）；

vz = q0q0 - q1q1 - q2q2 + q3q3；

ex =（ay*vz - az*vy）；

ey =（az*vx - ax*vz）；

ez =（ax*vy - ay*vx）；

halfT=0.002000；

gx = gx + ex*FACTOR/halfT； //校正陀螺儀測量值 用叉積誤差來做PI修正陀螺零偏

gy = gy + ey*FACTOR/halfT；

gz = gz + ez*FACTOR/halfT；

delta_2=（2*halfT*gx）*（2*halfT*gx）+（2*halfT*gy）*（2*halfT*gy）+（2*halfT*gz）*（2*halfT*gz）；

q0 =（1-delta_2/8）*q0 +（-q1*gx - q2*gy - q3*gz）*halfT；// 整合四元數率 四元數微分方程 四元數更新算法

q1 =（1-delta_2/8）*q1 +（q0*gx*halfT）+（q2*gz*halfT）-（q3*gy*halfT）

q2 =（1-delta_2/8）*q2 +（q0*gy*halfT）+（q2*gx*halfT）-（q3*gz*halfT）

q3 =（1-delta_2/8）*q3 +（q0*gz*halfT）+（q2*gy*halfT）-（q3*gx*halfT）

norm = sqrt（q0*q0 + q1*q1 + q2*q2 + q3*q3）；// 正常化四元

q0 = q0 / norm；

q1 = q1 / norm；

q2 = q2 / norm；

q3 = q3 / norm；// 化簡為歐拉角

Q_ANGLE.Pitch = asin（-2 * q1 * q3 + 2 * q0* q2）* 57.3；// pitch

Q_ANGLE.Roll = atan2（2 * q2 * q3 + 2 * q0 * q1，-2 * q1 * q1 - 2 * q2* q2 + 1）* 57.3；// roll

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

}

六、總結

該由STM32單片機設計的四旋翼飛行器，可以完成飛行，定高等基本功能，還可以進行部分拓展，使其具有更多功能和應用。并且該飛行器具有飛行穩定，成本較低，可擴展能力強等特點。但是在續航方面和精準度方面，還需進一步的深入和研究，使其具有更強的續航能力以及對飛行器的控制，將其應用于民用和軍用領域，低成本的無人機將產生一定的經濟價值和社會價值，推動四旋翼飛行器事業的發展。

參考文獻

[1]李朝清.單片機原理及接口技術.北京：北京航空航天大學出版社，1996.8

（作者單位：學校空軍預警學院）