陳國(guó)成，侯宗琰

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

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

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

1 四旋翼飛行器坐標(biāo)系的建立

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

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

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

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

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

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

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

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

2 互補(bǔ)濾波算法改進(jìn)

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

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

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

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

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

其中

互補(bǔ)濾波的具體步驟為

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

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

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

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

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

（5）將機(jī)體坐標(biāo)系下的重力向量和機(jī)體加速度計(jì)測(cè)量的重力向量進(jìn)行叉乘：

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

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

2.3 模糊控制

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

隸屬函數(shù)圖表示控制中輸入輸出所處的論域區(qū)間，如圖2，圖3，圖4所示。控制規(guī)則中，本文用E來(lái)表示無(wú)人機(jī)當(dāng)前時(shí)刻姿態(tài)角度與零度的偏差，“NB”表示“負(fù)角度很大”，“NS”表示“負(fù)角度小”，“0”表示“角度 0”，“PB”表示“正角度大”，“PS”表示“正角度小”；用EC來(lái)表示無(wú)人機(jī)當(dāng)前時(shí)刻姿態(tài)角的變化率也就是角速度，“NB”表示 “負(fù)角速度很大”，“NS”表示“負(fù)角速度小”，“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ù)具有計(jì)算方便的特點(diǎn)，需要注意的是，所獲得的反模糊是論域的確切值，而不是最終結(jié)果，還需要進(jìn)行去模糊化才能得到實(shí)際應(yīng)用的值。通過(guò)這樣的計(jì)算的最終結(jié)果，才能作為模糊控制。本文選擇通過(guò)MIN-MAX-重心法去模糊化。

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

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

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

至此，我們得到了模糊控制下的互補(bǔ)濾波算法原理圖見(jiàn)圖5。

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

3 試驗(yàn)與結(jié)果

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

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

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

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

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

試驗(yàn)通過(guò)編寫(xiě)飛控程序給予電調(diào)400 kHz頻率的PWM波形，當(dāng)占空比為1 ms～2 ms時(shí)，分別代表油門(mén)從0%～100%。陀螺儀輸出信號(hào)的低通濾波器的截止頻率為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。

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

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

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

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

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

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

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

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

4 結(jié)語(yǔ)

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