《電子技術(shù)應(yīng)用》
您所在的位置：首頁 > 嵌入式技術(shù) > 設(shè)計(jì)應(yīng)用 > 改進(jìn)擴(kuò)展卡爾曼濾波對(duì)四旋翼姿態(tài)解算的研究
改進(jìn)擴(kuò)展卡爾曼濾波對(duì)四旋翼姿態(tài)解算的研究
2017年電子技術(shù)應(yīng)用第10期
侯玉涵，王耀力
太原理工大學(xué) 信息工程學(xué)院,，山西 太原030024
摘要： 為解決四旋翼飛行器飛行控制中的穩(wěn)定性問題,，尤其是姿態(tài)解算精確性問題，提出改進(jìn)EKF算法,。該算法采用兩個(gè)EKF并行地對(duì)四旋翼飛行器的姿態(tài)數(shù)據(jù)進(jìn)行處理,，將改進(jìn)的EKF輸出的狀態(tài)變量和協(xié)方差加權(quán)后整合為飛行器的輸出狀態(tài)和協(xié)方差對(duì)飛行器姿態(tài)進(jìn)行實(shí)時(shí)解算。該算法有效地克服了非高斯白噪聲對(duì)飛行器姿態(tài)解算的影響,，減小了姿態(tài)數(shù)據(jù)的濾波偏差,。仿真表明，該算法較之普通EKF算法的濾波效果更好,，改進(jìn)EKF的均方誤差較之普通EKF降低了43.8%,，提高了系統(tǒng)的魯棒性。最后搭建以NI myRIO為核心控制器的四旋翼飛行器,，驗(yàn)證改進(jìn)EKF算法在四旋翼飛行器系統(tǒng)上的正確性和有效性,，且該算法亦能滿足飛行器實(shí)時(shí)控制的需求。
關(guān)鍵詞： 四旋翼飛行器 EKF 改進(jìn)EKF
中圖分類號(hào)： TP274.2
文獻(xiàn)標(biāo)識(shí)碼： A
DOI：10.16157/j.issn.0258-7998.171650
中文引用格式： 侯玉涵,，王耀力. 改進(jìn)擴(kuò)展卡爾曼濾波對(duì)四旋翼姿態(tài)解算的研究[J].電子技術(shù)應(yīng)用,，2017，43(10)：83-85,，93.
英文引用格式： Hou Yuhan,，Wang Yaoli. Research on attitude solution of four-rotor based on improved EKF[J].Application of Electronic Technique，2017,，43(10)：83-85,，93.
Research on attitude solution of four-rotor based on improved EKF
Hou Yuhan，Wang Yaoli
College of Information Engineering，Taiyuan University of Technology,，Taiyuan 030024,，China
Abstract： In order to solve the stability problem in flight control of four-rotor aircraft, especially the accuracy of attitude calculation. An improved EKF algorithm is proposed, the attitude data of the four-rotor are processed with two EKFs. And the state variables and the covariance are weighted and combined into the output state and covariance of the four-rotor to solve the attitude of the four-rotor in real time. The algorithm effectively overcomes the influence of non - Gaussian white noise on aircraft attitude calculation and reduces the filtering deviation of the attitude data. The simulation results show that the algorithm is better than the normal EKF algorithm, compared with normal EKF，the mean square error of improved EKF is reduced by 43.8%. The algorithm can effectively improve the robustness of the system. In the final test, the experiment is carried out on the four-rotor with NI myRIO as a core controller, and the correctness and validity of the improved EKF algorithm on the four- rotor system are verified, the algorithm can also satisfy the demand of real-time control.
Key words : four-rotor,；EKF,；improved extended Kalman

0 引言

    四旋翼飛行器因?yàn)槠浣Y(jié)構(gòu)簡單對(duì)稱、相對(duì)容易控制的特點(diǎn),，在監(jiān)測(cè),、檢查、救援,、電子情報(bào)等很多重要的領(lǐng)域被廣泛使用[1-2],，最近幾年在學(xué)術(shù)界和科研領(lǐng)域引起很大興趣。四軸飛行器姿態(tài)角解算的速度和精度直接影響飛行器是否能獲得實(shí)時(shí),、穩(wěn)定的飛行姿態(tài),，所以姿態(tài)解算是四旋翼飛行器研究的核心之一。

    對(duì)于四旋翼飛行器的姿態(tài)測(cè)量,，采用集成了三軸陀螺儀和三軸加速度計(jì)的MPU6050進(jìn)行姿態(tài)數(shù)據(jù)的采集,，在較簡單的硬件平臺(tái)下，應(yīng)該怎樣濾除外部干擾,，得到精確度比較高的數(shù)據(jù)變得十分重要,。近幾年姿態(tài)估計(jì)算法隨著電子技術(shù)的發(fā)展相繼出現(xiàn)。文獻(xiàn)[3]應(yīng)用互補(bǔ)濾波[3]對(duì)四旋翼的數(shù)據(jù)進(jìn)行融合,，該算法可以有效抑制因電機(jī)轉(zhuǎn)動(dòng)而帶來的周期性干擾和因溫度影響給MPU6050帶來的漂移誤差,。但是應(yīng)用互補(bǔ)濾波導(dǎo)致飛控系統(tǒng)有滯后現(xiàn)象，截止頻率附近衰減較慢,，處理后的數(shù)據(jù)存在誤差,，精確度較低。文獻(xiàn)[4]提出基于歐拉角的迭代擴(kuò)展卡爾曼[4]（IEKF）姿態(tài)測(cè)量方法,，該方法使用歐拉角方法描述姿態(tài)角,，基于EKF濾波算法，在計(jì)算觀測(cè)矩陣H時(shí)進(jìn)行多次迭代,，該算法估計(jì)出的姿態(tài)比普通EKF估計(jì)的姿態(tài)精度高,。但該算法復(fù)雜、計(jì)算量大,、循環(huán)周期長,，僅適用于仿真，在實(shí)際工程上應(yīng)用較少,。文獻(xiàn)[5-6]提出粒子濾波的算法,，粒子濾波算法是在狀態(tài)空間中尋找一組隨機(jī)樣本,，近似地表示出概率密度函數(shù)，獲得系統(tǒng)狀態(tài)的最小方差,，當(dāng)樣本數(shù)量趨于無窮時(shí)可以逼近任何形式的概率密度函數(shù),，擺脫了解決非線性濾波問題時(shí)隨機(jī)量必須滿足高斯分布的制約，因此粒子濾波能夠比較精準(zhǔn)地表達(dá)基于觀測(cè)量和控制量的后驗(yàn)分布,。但是粒子濾波需要大量的樣本數(shù)量才能較準(zhǔn)確地近似系統(tǒng)的后驗(yàn)概率密度,，使得算法復(fù)雜度較高，而且重采樣階段會(huì)造成樣本有效性和多樣性的損失,，導(dǎo)致樣本出現(xiàn)貧化現(xiàn)象,。

    利用MPU6050采集姿態(tài)信息，為了獲得較精確的姿態(tài)角,，對(duì)EKF算法進(jìn)行了改進(jìn),，MPU6050在每一個(gè)采樣時(shí)刻可以同時(shí)測(cè)得當(dāng)前時(shí)刻飛行器的姿態(tài)角和姿態(tài)角變化率的信息,，結(jié)合改進(jìn)EKF算法對(duì)數(shù)據(jù)進(jìn)行處理能夠準(zhǔn)確地估算飛行器當(dāng)前的位置和姿態(tài),。飛行器在飛行過程中受非高斯白噪聲的影響，本文在進(jìn)行姿態(tài)解算時(shí),，為有效模擬飛行器測(cè)量過程的噪聲,，用兩個(gè)加權(quán)高斯概率密度函數(shù)表示非高斯白噪聲函數(shù)，選擇合理高斯函數(shù)的權(quán)重,、均值和方差,。通過改進(jìn)型EKF有效地濾除該噪聲，減小了飛行器姿態(tài)偏差,。

1 系統(tǒng)建模

    為了獲得四旋翼飛行器的姿態(tài)數(shù)據(jù)信息,，首先選定兩個(gè)基本坐標(biāo)系：慣性坐標(biāo)系E(XYZ)和機(jī)體坐標(biāo)系B(X′Y′Z′)[7]，如圖1所示,。

ck4-t1.gif

ck4-gs1.gif

式中c代表cos,，s代表sin，轉(zhuǎn)換矩陣是非線性矩陣,，為避免該矩陣出現(xiàn)奇點(diǎn)問題,，飛行器在測(cè)試飛行過程中不會(huì)做出大幅度機(jī)動(dòng)動(dòng)作，因而可以認(rèn)為系統(tǒng)滿足以下假設(shè)：

    (1)四旋翼飛行器橫滾角與俯仰角滿足以下不等式：ck4-gs1-x1.gif

    (2)在實(shí)際飛行中飛行器角速度信號(hào)由陀螺儀測(cè)量,，因此可以假設(shè)飛行器橫滾角速度與偏航角速度為有界信號(hào)L∞,，即：wx∈L∞，wz∈L∞,。

    MPU6050可以快速,、穩(wěn)定地采集飛行器靜止時(shí)的姿態(tài)信息，但當(dāng)飛行器開始運(yùn)動(dòng)時(shí),，機(jī)體抖動(dòng)及螺旋槳高頻率的運(yùn)動(dòng)使得MPU6050測(cè)得的飛行器位姿存在明顯的偏差,。利用EKF將MPU6050采集的數(shù)據(jù)進(jìn)行融合,。EKF濾波原理是將非線性系統(tǒng)進(jìn)行線性化處理，利用泰勒級(jí)數(shù)展開系統(tǒng)函數(shù),，取一次項(xiàng)作為EKF的系數(shù)矩陣,，忽略了高階項(xiàng)噪聲。因此采用EKF進(jìn)行姿態(tài)解算存在較大的濾波偏差,。針對(duì)以上問題,，本文提出改進(jìn)型EKF，對(duì)四旋翼飛行器的姿態(tài)數(shù)據(jù)進(jìn)行解算,。

2 改進(jìn)型擴(kuò)展卡爾曼濾波原理及算法

    為有效濾除噪聲,，測(cè)得精確的姿態(tài)數(shù)據(jù)，對(duì)EKF濾波算法進(jìn)行了改進(jìn),，采用兩個(gè)EKF并行地對(duì)四旋翼飛行器的姿態(tài)數(shù)據(jù)進(jìn)行預(yù)測(cè)和更新,，將它們輸出的狀態(tài)變量加權(quán)整合后輸出飛行器的最優(yōu)狀態(tài)估計(jì)，因此經(jīng)改進(jìn)EKF處理后的飛行器姿態(tài)數(shù)據(jù)達(dá)到最小均方誤差,。實(shí)現(xiàn)對(duì)飛行器位姿的有效操控,，從理論上分析經(jīng)過改進(jìn)EKF濾波后的效果會(huì)更加接近真實(shí)值，有效地濾除噪聲,，且算法復(fù)雜度低,、運(yùn)算量小，不需要時(shí)頻域的頻繁轉(zhuǎn)換,，更不會(huì)因?yàn)闃颖緮?shù)量的選擇而導(dǎo)致樣本重采樣現(xiàn)象,，融合后飛行器姿態(tài)數(shù)據(jù)精度高。具體步驟如下：

    (1)EKF1 與EKF2 對(duì)四旋翼飛行器姿態(tài)數(shù)據(jù)并行地進(jìn)行處理,，輸出對(duì)應(yīng)的狀態(tài)變量X1,，k、X2,，k和協(xié)方差P1,，k、P2,，k,；

    (2)計(jì)算EKF1 和EKF2的似然函數(shù)，求出對(duì)應(yīng)的權(quán)重a1,、a2,；

    (3)加權(quán)整合后更新四旋翼飛行器狀態(tài)Xk和協(xié)方差Pk，輸出飛行器姿態(tài)的最優(yōu)估計(jì),；

    (4)實(shí)時(shí)調(diào)整飛行器姿態(tài),，待飛行器下一個(gè)狀態(tài)到達(dá)前返回步驟(1)，繼續(xù)下一次循環(huán),。

    具體過程如圖2所示,。

ck4-t2.gif

    對(duì)于一個(gè)隨機(jī)系統(tǒng),，具體的估計(jì)過程如下：

     ck4-gs2-4.gif

ck4-gs5-19.gif

3 仿真與實(shí)驗(yàn)

    在改進(jìn)型EKF理論的基礎(chǔ)上進(jìn)行仿真，在NI my RIO-1900的實(shí)驗(yàn)平臺(tái)上對(duì)飛行器進(jìn)行實(shí)驗(yàn),。

    為保證飛行器初始時(shí)刻的穩(wěn)定性及減小初始時(shí)刻姿態(tài)誤差,，對(duì)初始時(shí)刻四旋翼飛行器姿態(tài)數(shù)據(jù)進(jìn)行濾波處理，以橫滾角為例,，采集飛行器靜止時(shí)刻姿態(tài)數(shù)據(jù),，改進(jìn)EKF濾波后姿態(tài)與普通EKF濾波后姿態(tài)進(jìn)行對(duì)比，如圖3,、圖4所示,。普通EKF濾波后飛行器姿態(tài)有約1°左右的偏差，改進(jìn)EKF濾波后飛行器姿態(tài)穩(wěn)定性較好,，偏差較小,，幾乎可以忽略。二者相比較,，改進(jìn)型EKF的濾波效果較好,，魯棒性強(qiáng)。

ck4-t3.gif

ck4-t4.gif

    給飛行器橫滾角一個(gè)操控命令,，使其傾斜14°,，飛行器在執(zhí)行操控命令時(shí),，機(jī)體坐標(biāo)系繞慣性坐標(biāo)系旋轉(zhuǎn)會(huì)產(chǎn)生一定旋轉(zhuǎn)角,，橫滾角初始狀態(tài)不是絕對(duì)的0°，經(jīng)測(cè)試飛行器初始時(shí)刻橫滾角為0.41°,。如圖5和圖6所示,，飛行器接收到操控命令后，普通EKF濾波后的飛行器經(jīng)40 ms達(dá)到期望姿態(tài),，在140 ms 內(nèi)有波動(dòng),，140 ms 之后飛行器波動(dòng)范圍減小，但存在約0.5°左右的偏移,。改進(jìn)EKF濾波后的飛行器經(jīng)38 ms后達(dá)到期望姿態(tài),，僅用時(shí)80 ms飛行器就達(dá)到理想狀態(tài)，且偏移較小,，可以忽略,。從二者的對(duì)比圖可得出，改進(jìn)型EKF濾波后飛行器姿態(tài)角較穩(wěn)定,，提高了系統(tǒng)的準(zhǔn)確性和可靠性,。

ck4-t5.gif

ck4-t6.gif

    通過MPU6050采集四旋翼飛行器0～100 s內(nèi)的姿態(tài)數(shù)據(jù)，采集的姿態(tài)經(jīng)改進(jìn)EKF濾波后的動(dòng)態(tài)測(cè)試如圖7,，在4 s～16 s,、48 s～54 s,、60 s～70 s、72 s～86 s,、91 s～96 s時(shí)間段內(nèi),，改進(jìn)EKF估計(jì)值完全跟蹤到理想值，在16 s～18 s,、23 s～26 s,、56 s～59 s、96 s～99 s時(shí)間段內(nèi),，由于四旋翼飛行器姿態(tài)角度變化幅度較大,，改進(jìn)EKF估計(jì)值與理想值之間出現(xiàn)偏差，但在下一刻,，即19 s,、27 s、60 s,、100 s時(shí),，改進(jìn)EKF估計(jì)值快速恢復(fù)實(shí)時(shí)追蹤。

ck4-t7.gif

    經(jīng)計(jì)算,，改進(jìn)EKF估計(jì)值均方誤差為3.306,，普通EKF估計(jì)值均方誤差為5.884 3。改進(jìn)EKF估計(jì)值精度明顯高于普通EKF估計(jì)值精度,。精度提高的效率為：

    ck4-t7-x1.gif

    經(jīng)過多次測(cè)試可知：該算法設(shè)計(jì)合理,，姿態(tài)精度有較明顯的改善，改進(jìn)的算法已成功應(yīng)用在四旋翼飛行器上,，得到了理想的效果,。

4 結(jié)論

    本文改進(jìn)的擴(kuò)展卡爾曼了濾波算法，克服了非高斯白噪聲對(duì)傳感器姿態(tài)解算的影響,，且該算法復(fù)雜度低,、精度高、計(jì)算量小,。實(shí)驗(yàn)結(jié)果表明,，改進(jìn)的算法可以準(zhǔn)確地估計(jì)出姿態(tài)角，與經(jīng)過普通EKF濾波后的姿態(tài)測(cè)量值對(duì)比,，改進(jìn)EKF在飛行器姿態(tài)精度上有較明顯的改善,，驗(yàn)證改進(jìn)方法是有效的，為四旋翼無人機(jī)實(shí)時(shí)調(diào)整姿態(tài)提供了一種科學(xué)手段,。該算法只對(duì)MPU6050采集的數(shù)據(jù)進(jìn)行融合處理,，進(jìn)一步可將該算法應(yīng)用至地磁計(jì)模塊，對(duì)飛行器數(shù)據(jù)進(jìn)一步進(jìn)行融合處理,，以快速實(shí)現(xiàn)飛行器的垂直起飛和懸停的控制操作,。

參考文獻(xiàn)

[1] ROSALES C,，GANDOLFO D，SCAGLIA G,，et al.Trajectory tracking of a mini four-rotor helicopter in dynamic environments-a linear algebra approach[J].Robotica,，2015，33(8)：1628-1652.

[2] LUPASHIN S,，HEHN M,，MUELLER M W，et al.A platform for aerial robotics research and demonstration：the flying machine arena[J].Mechatronics,，2014,，24(1)：41-54.

[3] LU X，LI J,，LIU W.A study of complementary filter algorithm for four-rotor helicopters attitude control system[J].Sensors & Transducers,，2014，174(7)：298-306.

[4] 賀海鵬,，閻妍,，馬良，等.基于IEKF的四旋翼無人機(jī)姿態(tài)測(cè)量方法研究[J].計(jì)算機(jī)仿真,，2015(4)：56-60.

[5] MERCADO D A,，CASTILLO P，LOZANO R.Quadrotors data fusion using a particle filter[C].Unmanned Aircraft Systems(ICUAS),，2014 International Conference on.IEEE,，2014.

[6] 曲仕茹，馬志強(qiáng).改進(jìn)的粒子濾波在四旋翼姿態(tài)估計(jì)中的應(yīng)用[J].飛行力學(xué),，2013(5)：458-461,，466.

[7] ZHAO S，AN H,，LIU J，et al.Identification of aerodynamic parameter for quadrotor model[J].Journal of Electronic Measurement & Instrument,，2013,，27(8)：744-749.



作者信息：

侯玉涵，王耀力

(太原理工大學(xué) 信息工程學(xué)院,，山西 太原030024)

此內(nèi)容為AET網(wǎng)站原創(chuàng),，未經(jīng)授權(quán)禁止轉(zhuǎn)載。