一種基于模糊控制的自適應(yīng)互補濾波算法

陳國成，侯宗琰

（東北石油大學(xué) 電氣信息工程學(xué)院，大慶 163318）

四旋翼飛行器又稱為四軸飛行器（QUAD），其姿態(tài)解算是四軸飛行器能夠穩(wěn)定飛行的基礎(chǔ)，也是近年來國內(nèi)外高校和研究院所的研究熱點[1]。在現(xiàn)在的四軸飛行器上大都采用低成本捷聯(lián)慣性導(dǎo)航測量元件，其主要由陀螺儀、加速度計和磁力計組成。我們要獲得四軸的角度信息，就要對陀螺儀測得的角速度進行積分運算[2]。類似用在四軸飛行器上的這種低成本陀螺儀IMU器件易發(fā)散導(dǎo)致姿態(tài)漂移問題，需要進行慣性器件的慣導(dǎo)融合，其濾波融合才能獲得較為準確的三維姿態(tài)信息[3]。

鑒于此，有人提出采用卡爾曼濾波算法對數(shù)據(jù)進行濾波和融合以估計真實的姿態(tài)角[4]，并且還提出了基于四元數(shù)的擴展卡爾曼濾波器[5]以及梯度下降法[6]。然而，卡爾曼濾波器使用的觀測噪聲矩陣R和過程噪聲矩陣Q直接影響濾波效果，并且其準確值難以獲得[7]，擴展卡爾曼濾波也是近年來研究比較多的一種姿態(tài)解算的濾波算法[8]，但是該算法涉及到大量的矩陣運算，對飛控的處理器的性能要求較高[9]，梯度下降法計算量小，但是他的迭代步長是非動態(tài)的，其動態(tài)和靜態(tài)的估計性能難以兼顧，所以在實際應(yīng)用中并不是最好的選擇。綜上所述，針對本文對于互補濾波姿態(tài)解算算法中互補參數(shù)固定的情況下很難實現(xiàn)在所有的狀態(tài)下都能得到最優(yōu)的姿態(tài)估計，引入了模糊控制算法，把實時的姿態(tài)角和角速度作為模糊控制器的輸入進行互補參數(shù)實時的調(diào)整。

1 四旋翼飛行器坐標系的建立

我們想要解算出四軸飛行器的姿態(tài)，必須要建立導(dǎo)航坐標系和機體坐標系[10]。四軸飛行器一般采用“+”型或者“X”型，這里我們采用“X”型，使得飛行器更加靈活。

導(dǎo)航坐標系也稱為大地坐標系。起飛點是這個坐標系的原點。正北，正東和t天（北東天）構(gòu)成該坐標系的X，Y，Z軸。本文標記為坐標系R。以四軸飛行器的中心為起點，機頭方向為Y軸，從上往下看，X軸由機身右側(cè)確定。這個坐標系通常被稱為體坐標系。本文標記為坐標系r。機體坐標系隨著飛機姿態(tài)在空中變化而改變。這樣，我們可以使用歐拉角，四元數(shù)等來描述r和R之間的角位置關(guān)系。經(jīng)過以上設(shè)定，我們已經(jīng)初步建立了一個四軸飛機的數(shù)學(xué)模型。

這里我們用歐拉角來描述四軸飛行器的姿態(tài)。我們用歐拉角來表示飛機的姿態(tài)是直觀的，可以看做飛機繞固定軸的三次旋轉(zhuǎn)達到現(xiàn)在的姿態(tài)。

圖1 繞順序軸旋轉(zhuǎn)Fig.1 Rotating around a sequential axis

將機體坐標系置于地面坐標系中，我們可以根據(jù)空間矢量轉(zhuǎn)換得到飛行器的姿態(tài)。如圖1所示初始狀態(tài)為兩坐標原點重合，經(jīng)過3次平移（沿OX軸平移X，沿OY軸平移Y，沿OZ軸平移Z）和3次轉(zhuǎn)動（繞OX軸轉(zhuǎn)動φ，繞OY軸轉(zhuǎn)動θ，繞OZ軸轉(zhuǎn)動ψ）到達目標狀態(tài)。圖中繞XYZ軸分別對應(yīng)的名稱為俯仰角（pitch）、翻滾角（roll）和偏航角（yaw），對應(yīng)上圖中的φ、θ、ψ。按照X-Y-Z順序旋轉(zhuǎn)來進行分析，導(dǎo)航坐標系與機體坐標系的旋轉(zhuǎn)矩陣為

四元數(shù)可以描述三維空間中剛體的旋轉(zhuǎn)，既然四元數(shù)可用于描述旋轉(zhuǎn)，那么就可以用它來描述從導(dǎo)航坐標系R到機體坐標系r，也就是說四元數(shù)可以用來描述導(dǎo)航坐標系與機體坐標系的旋轉(zhuǎn)矩陣。

設(shè)描述四旋翼飛行器姿態(tài)的四元數(shù)[11]為

其中四元數(shù)q和方向余弦矩陣之間的關(guān)系式為

2 互補濾波算法改進

2.1 互補濾波姿態(tài)解算算法

先來簡單了解下什么是姿態(tài)解算。當某個矢量由不同的坐標系表示時，這個矢量在兩個坐標系中表示的大小和方向肯定是相同的。但是，由于這個旋轉(zhuǎn)矩陣是由融合前的四元數(shù)得到的，只要陀螺儀有積分誤差，這個由四元數(shù)得到的旋轉(zhuǎn)矩陣就會有誤差。當一個矢量經(jīng)過具有誤差的旋轉(zhuǎn)矩陣時，就會跟另一個坐標系中的理論值產(chǎn)生偏差。我們通過偏差來糾正這個旋轉(zhuǎn)矩陣。通過這個過程后，修正四元數(shù)以便修正姿態(tài)。

由于加速度計的特點是噪聲大，但是不會隨時間積累誤差。而陀螺儀的特點是長期比較穩(wěn)定，但會有積分誤差，互補濾波器根據(jù)它們的特性相互補充，并且每次用加速度計校準以校準陀螺儀?；パa濾波是指在短時間內(nèi)確定陀螺儀經(jīng)過角速度積分得到的角度作為基礎(chǔ)，并對加速度進行低通濾波并采樣得到的加速度值進行平均，以校正陀螺儀獲得的角度。在短時間內(nèi)，使用陀螺儀更為準確，長時間使用加速度計會更準確，這時如果增加了加速度計的比例，這就是互補。但這兩個傳感器的數(shù)據(jù)不能直接使用，因為這兩個傳感器的特性，加速度計需要濾除高頻噪聲，陀螺儀要濾除低頻噪聲?；パa濾波器基于傳感器的特性，通過低通或者高通濾波器，然后叉乘得出差值加權(quán)補償四元數(shù)，以得到較為準確的姿態(tài)解算結(jié)果。

基于以上論述我們進一步可得到基于四元數(shù)互補濾波的傳遞函數(shù)和算法步驟：

互補濾波器傳遞函數(shù)：

其中

互補濾波的具體步驟為

（1）初始化四元數(shù)

姿態(tài)解算開始時，將已知載體初始姿態(tài)角帶入下式，求出初始時刻四元數(shù)。

（2）通過傳感器得到角速度，加速度，磁力計值分別讀取加速度計X，Y，Z軸上的測量值ax，ay，az；陀螺儀三軸測量值wx，wy，wz；磁力計三軸測量值mx，my，mz。

（3）將加速度計和磁力計讀取到的數(shù)據(jù)轉(zhuǎn)化為單位向量。

（4）從四元數(shù)里獲取在機體坐標系下的重力向量：

（5）將機體坐標系下的重力向量和機體加速度計測量的重力向量進行叉乘：

2.2 基于模糊算法的自適應(yīng)互補濾波估計

針對互補濾波的缺點，既濾波器PI控制器的互補參數(shù)kp和ki是固定的情況下很難滿足實現(xiàn)在所有狀態(tài)下都能得到最優(yōu)的系統(tǒng)估計，互補濾波的補充前提條件是加速度計的測量僅僅是重力加速度，所以當飛機的俯仰角和滾轉(zhuǎn)角很大時，證明飛機正在加速飛行，所以測得的加速度傳感器測量得到的加速度計這個時候就不僅僅是重力加速度還有運動方向的加速度。這些加速度分量都會影響互補濾波的精度。所以這個時候加速度計所測量所得的數(shù)據(jù)就不能用來校正陀螺儀積分所帶來的誤差或者說減小加速度計校正誤差時的比重。因此引入模糊控制算法目的就在于參數(shù)能夠在不同的運動狀態(tài)下自適應(yīng)調(diào)節(jié)，實現(xiàn)自適應(yīng)互補濾波，因為在積分項上我們加入了積分分離和積分限幅等算法，所以本文只針對kp進行模糊自適應(yīng)調(diào)節(jié)?；驹瓌t就是在大機動大過載的情況下，比例參數(shù)能夠自動減小也就是信任陀螺儀多一點，在平穩(wěn)的時候比例參數(shù)增加也就是信任加速度計多一些。

2.3 模糊控制

2.3.1 建立模糊控制規(guī)則

隸屬函數(shù)圖表示控制中輸入輸出所處的論域區(qū)間，如圖2，圖3，圖4所示?？刂埔?guī)則中，本文用E來表示無人機當前時刻姿態(tài)角度與零度的偏差，“NB”表示“負角度很大”，“NS”表示“負角度小”，“0”表示“角度 0”，“PB”表示“正角度大”，“PS”表示“正角度小”；用EC來表示無人機當前時刻姿態(tài)角的變化率也就是角速度，“NB”表示 “負角速度很大”，“NS”表示“負角速度小”，“0”表示“角速度為 0”，“PB”表示“正角速度大”，“PS”表示“正角速度小”。

圖2 偏差角變化率隸屬度函數(shù)Fig.2 Variation angle change rate membership function

圖3 偏差角隸屬度函數(shù)Fig.3 Offset angle membership function

圖4 輸出屬度函數(shù)Fig.4 Output membership function

三角形隸屬函數(shù)具有計算方便的特點，需要注意的是，所獲得的反模糊是論域的確切值，而不是最終結(jié)果，還需要進行去模糊化才能得到實際應(yīng)用的值。通過這樣的計算的最終結(jié)果，才能作為模糊控制。本文選擇通過MIN-MAX-重心法去模糊化。

2.3.2 建立模糊控制規(guī)則

建立模糊控制規(guī)則的原則就是當無人機在大擾動的情況下即當四旋翼飛行器的俯仰角或者翻滾角大于某一值時陀螺儀的比值占的比例多一點，相反則加速度計占的比例大，當四旋翼飛行器的俯仰角或者翻滾角的角速度在增大的時候陀螺儀的比值增大。相反，當四旋翼飛行器的俯仰角或者翻滾角的角速度在減小的時候，加速度計的比值增大。模糊規(guī)則如下如表1所示。

表1 kp模糊規(guī)則表Tab.1 Fuzzy rules table of kp

至此，我們得到了模糊控制下的互補濾波算法原理圖見圖5。

圖5 自適應(yīng)互補濾波原理圖Fig.5 Adaptive complementary filtering schematic

3 試驗與結(jié)果

本文搭建了F450大疆機架的試驗平臺，對角距離為450 mm。為獲得更加靈敏和穩(wěn)定的飛行效果，飛機采用“X”型飛行模式控制。電調(diào)采用好贏40A，電機采用郎宇A(yù)2212 980kV的直流無刷電機，槳葉采用ATG 1047槳飛控板為自行設(shè)計焊接的飛行控制器。整個飛控代碼工程自行編寫。實物圖如圖6、圖7所示。

IMU慣性導(dǎo)航裝置使用MPU6050傳感器，該裝置包含一個三軸陀螺儀和三軸加速計以支持I2C接口以及一個可編程低通濾波器。本文加上三軸磁力計用于糾正偏航角。但不采用互補濾波算法，所以本文對航向角不做討論。

圖6 四旋翼飛行器實物圖Fig.6 Physical effects of four-rotor aircraft

圖7 四旋翼飛行器室飛行效果圖Fig.7 Flight effect diagram of quadrotor aircraft room

飛行控制單元的主控芯片采用基于ARM Cortex-M4內(nèi)核的STM32f407。通過該芯片我們軟件模擬I2C，通過硬件SPI接口并且使用通用定時器可輸出PWM波。我們編程輸出400 Hz的PWM波給電機調(diào)速器用于電機調(diào)速。外部采用8 MHz的外部高速晶振，通過配置寄存器分頻使MCU的工作頻率達到168 MHz，完全可以滿足四軸飛行器的控制。

試驗通過編寫飛控程序給予電調(diào)400 kHz頻率的PWM波形，當占空比為1 ms～2 ms時，分別代表油門從0%～100%。陀螺儀輸出信號的低通濾波器的截止頻率為100 Hz，陀螺儀的輸出量程為±2000°/s，更新周期為 0.005 s。翻滾角 PID參數(shù)為0.6，0.1，2.2，俯仰角 PID 參數(shù)為 0.6，0.1，2.2，偏航角PID 參數(shù)為 1.0，0.1，2.0。

試驗采用非加速運動測試和加速運動測試，采樣頻率為5 Hz，數(shù)據(jù)如圖8、圖9所示。

圖8 非加速運動狀態(tài)下俯仰角姿態(tài)Fig.8 Pitch angle attitude in non-accelerated motion

圖9 非加速運動狀態(tài)下翻滾角姿態(tài)Fig.9 Roll angle attitude in non-accelerated motion

四旋翼飛行器不做水平方向的加速度運動，將飛行器分別繞機體坐標的X軸和Y軸進行轉(zhuǎn)動，經(jīng)過一段時間的數(shù)據(jù)采集，我們發(fā)現(xiàn)原始互補濾波得到的姿態(tài)角跟改進的互補濾波姿態(tài)角幾乎保持一致。都可以很好地解算出機體當前的俯仰角和翻滾角，并且當靜止飛行器的時候兩者都能迅速回歸零度。

接下來我們讓四旋翼飛行器保持一個傾角并進行加速度運動，以此來模擬飛行器空中的加速度飛行。得到一個姿態(tài)角數(shù)據(jù)將其減去四旋翼飛行中保持的傾角度數(shù)這樣我們得到一個姿態(tài)解算誤差，數(shù)據(jù)如圖10、圖11所示。

圖10 加速運動狀態(tài)下俯仰角姿態(tài)誤差Fig.10 Pitch angle gesture in accelerated motion

圖11 加速運動狀態(tài)下翻滾角姿態(tài)誤差Fig.11 Roll angle attitude in accelerated motion

這里我們看俯仰角和橫滾角的姿態(tài)解算情況，此時加速度計測得的加速度不再只是重力加速度，還包括水平運動方向上的加速度分量。通過圖我們可以看出原始互補濾波解算出的姿態(tài)角較改進的互補濾波解算出的姿態(tài)角有較大的誤差。通過比較可以看出，改進的互補濾波解算出的姿態(tài)角更為準確。

4 結(jié)語

分析了互補濾波姿態(tài)計算算法的原理和實現(xiàn)方法。指出了加速度對四旋翼姿態(tài)解算計算的影響。在水平加速度較大大時提出了改進的互補濾波算法，指出了互補濾波的限制條件和缺陷。本文設(shè)計了一種加入模糊控制的改進互補濾波算法，實現(xiàn)了在四旋翼水平加速運動的狀態(tài)下，四旋翼飛行器仍能很好的進行準確的姿態(tài)解算。試驗結(jié)果表明：該算法提高了姿態(tài)計算的準確性，具有良好的實時性。適用于四旋翼飛行器的姿態(tài)計算。