基于擴(kuò)展卡爾曼濾波的多旋翼飛行器融合姿態(tài)解算算法

丁 磊，吳蔚劼，方 挺

(1.安徽工業(yè)大學(xué)電氣與信息工程學(xué)院，安徽馬鞍山243000；2.西安電子科技大學(xué)通信工程學(xué)院，陜西西安710126)

多旋翼飛行器是一種能夠自由起降，且能穩(wěn)定懸停于空中的六自由度飛行器。該飛行器起飛不受場(chǎng)地限制，可在任意場(chǎng)合下工作，甚至可用于執(zhí)行各種高危任務(wù)，故其在偵察敵情、災(zāi)區(qū)救援、道路監(jiān)控、農(nóng)業(yè)植保、電力巡線等領(lǐng)域有較好的應(yīng)用前景[1]。姿態(tài)控制是飛行器總體設(shè)計(jì)的重要組成部分，而姿態(tài)解算是飛行控制系統(tǒng)的核心。飛行器具有欠驅(qū)動(dòng)、多變量等比較復(fù)雜的特性[2]，其穩(wěn)定飛行與靈活控制對(duì)實(shí)時(shí)姿態(tài)解算的要求更高[3]，故飛行器姿態(tài)角的實(shí)時(shí)解算算法是多旋翼飛行器研究的熱點(diǎn)之一。

利用慣性導(dǎo)航測(cè)量器件如加速度計(jì)、陀螺儀和電子羅盤可快速、穩(wěn)定地測(cè)量飛行器的當(dāng)前姿態(tài)角，但多旋翼飛行器飛行時(shí)電機(jī)高速轉(zhuǎn)動(dòng)，機(jī)身隨之產(chǎn)生高頻振動(dòng)，加速度計(jì)對(duì)機(jī)身振動(dòng)異常敏感，致使其測(cè)量數(shù)據(jù)中摻雜大量高頻噪聲，導(dǎo)致后續(xù)姿態(tài)解算的結(jié)果產(chǎn)生較大誤差。常用飛行器飛行姿態(tài)的解算算法有歐拉角、方向余弦、四元數(shù)互補(bǔ)濾波算法等。其中：歐拉角法解析過(guò)程比較簡(jiǎn)單，但包含大量的三角函數(shù)運(yùn)算，計(jì)算時(shí)間較長(zhǎng)，且當(dāng)俯仰角轉(zhuǎn)動(dòng)90°時(shí)會(huì)出現(xiàn)萬(wàn)向鎖現(xiàn)象，不能進(jìn)行全姿態(tài)解算[4]；方向余弦法可避免萬(wàn)向鎖現(xiàn)象的發(fā)生，能夠進(jìn)行全姿態(tài)的解算，但姿態(tài)矩陣微分方程是線性微分方程并含9個(gè)未知量，計(jì)算量大，實(shí)時(shí)性較差[5]；四元數(shù)法僅含4個(gè)未知量，計(jì)算量小且算法易編程實(shí)現(xiàn)，可進(jìn)行全姿態(tài)的解算[6]；互補(bǔ)濾波算法是一種數(shù)據(jù)融合算法，其根據(jù)傳感器的不同特性，通過(guò)融合低通濾波和高通濾波接近真實(shí)信號(hào)[7]。徑向基(radial basis function，RBF)神經(jīng)網(wǎng)絡(luò)是一種局部逼近網(wǎng)絡(luò)，具全局唯一最佳逼近的特性，且其結(jié)構(gòu)簡(jiǎn)單、訓(xùn)練速度快[8-9]。鑒于此，以四元數(shù)互補(bǔ)濾波算法解算的多旋翼飛行器姿態(tài)角作為參考向量，采用RBF神經(jīng)網(wǎng)絡(luò)設(shè)計(jì)加速度計(jì)非線性高頻濾波器，用擴(kuò)展卡爾曼濾波算法實(shí)時(shí)解算飛行器的姿態(tài)角，以期抑制機(jī)身振動(dòng)對(duì)加速度噪聲的干擾，提高飛行器姿態(tài)角的解算精度。

1 姿態(tài)解算

1.1 慣性導(dǎo)航

假設(shè)飛行器坐標(biāo)系為Hb，大地坐標(biāo)系為Hc，將飛行器坐標(biāo)系分解成按Z-Y-X軸順序轉(zhuǎn)動(dòng)，3次轉(zhuǎn)動(dòng)均繞飛行器坐標(biāo)系的某一軸。首先飛行器繞大地坐標(biāo)系Z軸，以右手系轉(zhuǎn)動(dòng)角度ψ得到飛行器新的坐標(biāo)系，ψ為飛行器的偏航角Yaw。繞Z軸定軸轉(zhuǎn)動(dòng)的基本旋轉(zhuǎn)矩陣為

再繞新的機(jī)體坐標(biāo)系Y軸，以右手系轉(zhuǎn)動(dòng)角度θ，得到飛行器新的坐標(biāo)系。θ為飛行器的俯仰角Pitch，繞Y軸定軸轉(zhuǎn)動(dòng)的基本旋轉(zhuǎn)矩陣為

最后繞新的機(jī)體坐標(biāo)系X軸，以右手系轉(zhuǎn)動(dòng)角度φ，得到飛行器的最終坐標(biāo)系。φ為飛行器的橫滾角Roll，繞X軸定軸轉(zhuǎn)動(dòng)的基本旋轉(zhuǎn)矩陣為

3次分解的動(dòng)作可視為飛行器從初始位置到最終位置的一次性等效旋轉(zhuǎn)。按歐拉角Z-Y-X依次進(jìn)行轉(zhuǎn)動(dòng)，令旋轉(zhuǎn)矩陣為，則

由式(4)旋轉(zhuǎn)矩陣可直接解算出飛行器的偏航角ψ、俯仰角θ和橫滾角φ，如

1.2 四元數(shù)互補(bǔ)濾波算法

1.2.1 姿態(tài)角的四元數(shù)法表示

從式(7)可獲得飛行器的偏航角ψ、俯仰角θ和橫滾角φ。

1.2.2 互補(bǔ)濾波算法

對(duì)于6軸運(yùn)動(dòng)處理組件MPU6050，加速度計(jì)對(duì)多旋翼飛行器的加速度較為敏感，用加速度計(jì)的實(shí)時(shí)值計(jì)算飛行器當(dāng)前傾角誤差較大；陀螺儀經(jīng)過(guò)積分得到的角度不受加速度計(jì)的影響，但隨時(shí)間的推移，其積分漂移和溫漂會(huì)引起較大誤差[11-13]。

圖1 互補(bǔ)濾波原理Fig.1 Principle of complementary filter

根據(jù)互補(bǔ)濾波的工作原理(如圖1)，假設(shè)加速度計(jì)的測(cè)量數(shù)據(jù)為t1=r+e+u1，陀螺儀的測(cè)量數(shù)據(jù)為t2=r+e+u2，其中r為真實(shí)數(shù)據(jù)；e為傳感器本身的誤差；u1，u2分別為高頻噪聲和低頻噪聲。設(shè)傳遞函數(shù)之和為1，為低通濾波器，用來(lái)濾除加速度計(jì)的高頻噪聲；為高通濾波器，用來(lái)濾除陀螺儀的低頻噪聲。通過(guò)互補(bǔ)濾波得到估計(jì)傳遞函數(shù)：

其中：R(s)和E(s)為全通量，包含真實(shí)數(shù)據(jù)和傳感器本身的誤差，而加速度計(jì)中所含的高頻噪聲信號(hào)U1(s)被H1(s)低通濾波器濾除，只容許低頻信號(hào)通過(guò)；陀螺儀所含的低頻噪聲信號(hào)U2(s)被H2(s)高通濾波器濾除，只容許高頻信號(hào)通過(guò)?；パa(bǔ)濾波算法根據(jù)陀螺儀、加速度計(jì)的不同特性，短時(shí)間內(nèi)采用陀螺儀的數(shù)據(jù)作為最優(yōu)參考，同時(shí)對(duì)加速度計(jì)的數(shù)據(jù)進(jìn)行滑動(dòng)窗口濾波處理，用其得到的值不斷地對(duì)陀螺儀的數(shù)據(jù)進(jìn)行校正[14-15]，通過(guò)融合低通濾波和高通濾波計(jì)算出接近真實(shí)信號(hào)的數(shù)據(jù)，供后續(xù)飛行器姿態(tài)解算使用。

2 RBF神經(jīng)網(wǎng)絡(luò)非線性濾波

RBF神經(jīng)網(wǎng)絡(luò)輸入層與隱藏層間的映射為非線性關(guān)系，隱藏層與輸出層間的映射為線性關(guān)系[16]，其網(wǎng)絡(luò)結(jié)構(gòu)如圖2。RBF神經(jīng)網(wǎng)絡(luò)隱藏層神經(jīng)元的輸出為

其中：X為含有噪聲的m維加速度計(jì)數(shù)據(jù)，作為輸入向量；Ci為第i個(gè)神經(jīng)元基函數(shù)的中心；σi為第i個(gè)神經(jīng)元的基寬；Gi為高斯徑向基函數(shù)。

圖2 RBF神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)Fig.2 Structure of RBF neural network

RBF神經(jīng)網(wǎng)絡(luò)輸出層的輸出yj為

其中：Wij為隱藏層第i個(gè)神經(jīng)元與輸出層第j個(gè)神經(jīng)元間的權(quán)重；bj為輸出層第j個(gè)神經(jīng)元的閾值。

RBF神經(jīng)網(wǎng)絡(luò)辨識(shí)器的性能評(píng)價(jià)函數(shù)J(k)為

其中：k為任一時(shí)刻；yj(k)為實(shí)際輸出；為網(wǎng)絡(luò)輸出，經(jīng)過(guò)網(wǎng)絡(luò)訓(xùn)練，使得J越小越好，滿足J≤ε的條件，ε為一個(gè)極小的正數(shù)。

RBF神經(jīng)網(wǎng)絡(luò)中心的選取采用自組織學(xué)習(xí)法，通過(guò)監(jiān)督學(xué)習(xí)訓(xùn)練隱藏層與輸出層間的權(quán)值和閾值[17]。網(wǎng)絡(luò)的權(quán)值經(jīng)由梯度下降算法計(jì)算不斷改變，能夠快速收斂到全局最優(yōu)點(diǎn)。權(quán)值的更新算法為

其中a為學(xué)習(xí)速率，文中取0.1。學(xué)習(xí)速率太大則無(wú)法快速逼近最優(yōu)值，會(huì)在其附近來(lái)回振蕩；太小則收斂速度太慢。

非線性濾波器的權(quán)值系數(shù)含實(shí)時(shí)性的問(wèn)題，需能夠?qū)崟r(shí)、快速求解。系統(tǒng)的非線性差分方程可表示為

式中：p為當(dāng)前時(shí)刻，p+1為下一時(shí)刻，前者為系統(tǒng)的狀態(tài)方程；后者為系統(tǒng)的觀測(cè)方程；f，h為非線性函數(shù)；v和w為白噪聲序列且均值為0。非線性濾波的最優(yōu)值是通過(guò)y(p)來(lái)估計(jì)出系統(tǒng)的(p)，并使其是x(p)的最優(yōu)估計(jì)。RBF神經(jīng)網(wǎng)絡(luò)實(shí)際上是系統(tǒng)對(duì)f和h兩個(gè)非線性函數(shù)的最佳逼近。

圖3 加速度計(jì)RBF神經(jīng)網(wǎng)絡(luò)非線性濾波基本原理Fig.3 Basic principle of nonlinear filtering of accelerometer RBF neural network

利用RBF神經(jīng)網(wǎng)絡(luò)對(duì)加速度計(jì)姿態(tài)解算的角度值進(jìn)行非線性濾波的基本原理如圖3。圖3中加速度計(jì)解算的角度作為輸入信號(hào)，該信號(hào)包含真值X與噪聲ε，參考信號(hào)d為互補(bǔ)濾波姿態(tài)解算算法解算出的角度，經(jīng)RBF神經(jīng)網(wǎng)絡(luò)非線性濾波算法得到輸出信號(hào)Y。

3 擴(kuò)展卡爾曼濾波融合姿態(tài)解算算法

多旋翼飛行器系統(tǒng)為非線性系統(tǒng)，傳統(tǒng)的卡爾曼濾波算法只適用于線性系統(tǒng)，故文中采用擴(kuò)展卡爾曼濾波算法(EKF)解決多旋翼飛行器系統(tǒng)的非線性問(wèn)題[18-20]。擴(kuò)展卡爾曼濾波算法將系統(tǒng)的非線性部分進(jìn)行泰勒級(jí)數(shù)展開(kāi)，忽略其高階無(wú)窮小部分，同時(shí)保留系統(tǒng)的線性部分，最終得到近似模型，將其非線性問(wèn)題轉(zhuǎn)化為線性問(wèn)題[21-23]。其通過(guò)上一時(shí)刻的角度X(k)預(yù)測(cè)更新下一時(shí)刻的角度X(k-1)，是一種典型的遞推算法，包括系統(tǒng)狀態(tài)方程和觀測(cè)方程的建立。

建立系統(tǒng)的狀態(tài)方程，如

其中W(k-1)為系統(tǒng)的噪聲向量，選取互補(bǔ)濾波姿態(tài)解算算法解算出的姿態(tài)角為系統(tǒng)的狀態(tài)量，誤差較小。

建立系統(tǒng)的觀測(cè)方程，如

式中：H為單位矩陣；Vk為觀測(cè)的測(cè)量噪聲。將經(jīng)RBF神經(jīng)網(wǎng)絡(luò)非線性濾波后的加速度計(jì)解算的角度作為觀測(cè)向量。

之后，通過(guò)以下步驟完成系統(tǒng)狀態(tài)的預(yù)測(cè)與更新[24-25]。

1)狀態(tài)的預(yù)測(cè)

式中：A為系統(tǒng)參數(shù)矩陣；X(k|k-1)為利用上一時(shí)刻狀態(tài)預(yù)測(cè)的結(jié)果；X(k-1|k-1)為上一時(shí)刻的最優(yōu)預(yù)測(cè)值；B為控制參數(shù)矩陣；U(k)為當(dāng)前狀態(tài)的控制量。通過(guò)上一時(shí)刻的最優(yōu)預(yù)測(cè)值預(yù)測(cè)下一時(shí)刻的狀態(tài)值。

2)協(xié)方差矩陣的預(yù)測(cè)

其中P(k|k-1)為X(k|k-1)對(duì)應(yīng)的協(xié)方差矩陣，同時(shí)也是P(k-1|k-1)協(xié)方差矩陣的估計(jì)，Q為系統(tǒng)噪聲。

3)卡爾曼增益的更新

式中：Kg(k)為卡爾曼增益；R為觀測(cè)噪聲的協(xié)方差矩陣。

4)狀態(tài)的更新

其中Z(k)為觀測(cè)矩陣。

5)協(xié)方差矩陣的更新

其中E為單位矩陣。

擴(kuò)展卡爾曼濾波融合姿態(tài)解算算法的原理圖如圖4。

圖4 擴(kuò)展卡爾曼濾波融合姿態(tài)解算算法原理Fig.4 Principle of extended Kalman filter fusion attitude solving algorithm

4 實(shí)驗(yàn)仿真分析

基于STM32單片機(jī)、MPU6050六軸運(yùn)動(dòng)傳感器及HMC5883電子羅盤，組成飛行器姿態(tài)解算硬件系統(tǒng)，利用提出的四元數(shù)互補(bǔ)濾波姿態(tài)解算算法對(duì)飛行器的俯仰角、橫滾角、偏航角分別進(jìn)行靜態(tài)和動(dòng)態(tài)的實(shí)時(shí)解算。偏航角的解算需用到電子羅盤，利用加速度計(jì)只能單獨(dú)進(jìn)行俯仰角及橫滾角靜態(tài)和動(dòng)態(tài)兩種情況的解算，之后采用RBF神經(jīng)網(wǎng)絡(luò)對(duì)加速度計(jì)解算的姿態(tài)角進(jìn)行非線性高頻濾波，其中徑向基函數(shù)的擴(kuò)展系數(shù)取1.0。最后通過(guò)擴(kuò)展卡爾曼濾波融合姿態(tài)解算算法解算出飛行器靜態(tài)和動(dòng)態(tài)的俯仰角和橫滾角。系統(tǒng)每2 ms進(jìn)行1次姿態(tài)角解算?；パa(bǔ)濾波姿態(tài)算法解算的結(jié)果如圖5，6。加速度計(jì)姿態(tài)角濾波前的結(jié)果如圖7。加速度計(jì)解算的姿態(tài)角經(jīng)RBF濾波后的結(jié)果如圖8。擴(kuò)展卡爾濾波解算的姿態(tài)角靜態(tài)動(dòng)態(tài)結(jié)果如圖9，10。

從圖5可看出：互補(bǔ)濾波姿態(tài)解算算法解算出的靜態(tài)姿態(tài)角效果良好，俯仰角的解算范圍在(-0.23°，+0.23°)，橫滾角的解算范圍在(-0.2°，+0.27°)，經(jīng)過(guò)水平校準(zhǔn)后，可基本保持在0°附近，且在長(zhǎng)時(shí)間測(cè)量過(guò)程中角度沒(méi)有發(fā)散；偏航角的解算幅度在0.5°以內(nèi)，長(zhǎng)時(shí)間范圍內(nèi)穩(wěn)定、不發(fā)散。從圖6可看出，互補(bǔ)濾波姿態(tài)解算算法解算出的動(dòng)態(tài)姿態(tài)角數(shù)據(jù)曲線有少許毛刺，但從總體效果來(lái)看，姿態(tài)角的解算效果實(shí)時(shí)性強(qiáng)、不發(fā)散，姿態(tài)跟蹤速度快，能夠滿足基本日常飛行需求。從圖7可看出，加速度計(jì)僅通過(guò)滑動(dòng)窗口濾波，因高頻噪聲的干擾導(dǎo)致姿態(tài)角波動(dòng)范圍較大，靜態(tài)時(shí)的角度波動(dòng)范圍達(dá)到2°，動(dòng)態(tài)時(shí)有尖峰產(chǎn)生，波形毛刺較多，測(cè)量誤差較大。從圖8可看出：加速度計(jì)解算的姿態(tài)角經(jīng)RBF神經(jīng)網(wǎng)絡(luò)非線性高頻濾波，靜態(tài)測(cè)量的姿態(tài)角較為穩(wěn)定，趨近于0°且高頻噪聲得到有效濾除；姿態(tài)角振幅在0.2°左右，動(dòng)態(tài)測(cè)量的姿態(tài)角與濾波前相比，濾波效果較為明顯，尖峰部分得到一定程度減弱，高頻噪聲得到有效抑制，數(shù)據(jù)波形更加穩(wěn)定、光滑。

圖6 互補(bǔ)濾波姿態(tài)解算動(dòng)態(tài)結(jié)果Fig.6 Dynamic results of complementary filtering attitude solution

圖7 加速度計(jì)姿態(tài)解算濾波前的結(jié)果Fig.7 Results of accelerometer attitude solution before filtering

圖9 擴(kuò)展卡爾曼濾波解算的姿態(tài)角靜態(tài)結(jié)果Fig.9 Static results of the attitude angle calculated by extended Kalman filter

圖10 擴(kuò)展卡爾曼濾波融合姿態(tài)解算的姿態(tài)角動(dòng)態(tài)結(jié)果Fig.10 Dynamic results of the attitude angle of extended Kalman filter fusion attitude solution

從圖9，10可看出：擴(kuò)展卡爾曼濾波融合姿態(tài)解算的效果比互補(bǔ)濾波解算的效果更好、精度更高，靜態(tài)時(shí)，俯仰角的解算范圍在(-0.07°，+0.13°)，橫滾角的解算范圍在(-0.04°，+0.15°)，姿態(tài)角的解算幅度在0.2°以內(nèi)，解算精度有很大提高；與互補(bǔ)濾波解算效果相比，動(dòng)態(tài)時(shí)，解算的姿態(tài)角曲線更為平滑，毛刺部分得到有效消除，穩(wěn)定性更高，響應(yīng)速度和收斂速度更快，能夠?qū)崟r(shí)追蹤飛行器的當(dāng)前姿態(tài)角。

5 結(jié) 論

針對(duì)飛行器飛行時(shí)機(jī)身抖動(dòng)對(duì)加速度計(jì)傳感器產(chǎn)生較大的高頻噪聲干擾，采用基于RBF神經(jīng)網(wǎng)絡(luò)的非線性高頻濾波算法，并結(jié)合互補(bǔ)濾波算法對(duì)飛行器的姿態(tài)角進(jìn)行擴(kuò)展卡爾曼濾波的融合姿態(tài)解算。實(shí)驗(yàn)表明：擴(kuò)展卡爾曼濾波的融合姿態(tài)解算算法對(duì)高頻噪聲有很好的抑制作用，提高了加速度計(jì)對(duì)姿態(tài)的測(cè)量精度以及飛行控制系統(tǒng)中姿態(tài)解算算法的解算精度，使得解算出的姿態(tài)角精度得到有效提高且在長(zhǎng)時(shí)間范圍內(nèi)穩(wěn)定性更強(qiáng)。