丁 磊，吳蔚劼，方 挺

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

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

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

1 姿態(tài)解算

1.1 慣性導航

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

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

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

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

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

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

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

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

1.2.2 互補濾波算法

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

圖1 互補濾波原理Fig.1 Principle of complementary filter

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

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

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

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

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

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

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

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

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

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

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

其中a為學習速率，文中取0.1。學習速率太大則無法快速逼近最優(yōu)值，會在其附近來回振蕩；太小則收斂速度太慢。

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

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

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

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

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

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

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

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

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

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

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

1)狀態(tài)的預測

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

2)協(xié)方差矩陣的預測

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

3)卡爾曼增益的更新

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

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

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

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

其中E為單位矩陣。

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

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

4 實驗仿真分析

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

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

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

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

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

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

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

5 結(jié) 論

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