喬美英,高翼飛,李宛妮,姚文豪
(河南理工大學(xué) 電氣工程與自動(dòng)化學(xué)院,焦作 454000)
隨著無(wú)人機(jī)、虛擬現(xiàn)實(shí)器件(Virtual Reality, VR)、可穿戴醫(yī)療設(shè)備等技術(shù)的不斷發(fā)展,低成本、小體積、低功耗的微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System, MEMS)慣導(dǎo)設(shè)備也得到了廣泛的發(fā)展應(yīng)用[1,2]。其內(nèi)部慣性測(cè)量單元(Inertial Measurement Unit,IMU)是實(shí)現(xiàn)慣性導(dǎo)航的基本組成單元,根據(jù)MEMS-IMU系統(tǒng)不同傳感器的測(cè)量數(shù)據(jù),通過(guò)算法融合并實(shí)時(shí)解算出的姿態(tài)參數(shù),實(shí)現(xiàn)對(duì)載體的定位和跟蹤。因此快速、準(zhǔn)確地解算載體實(shí)時(shí)的姿態(tài)信息是實(shí)現(xiàn)慣性導(dǎo)航的關(guān)鍵。
目前對(duì)慣性導(dǎo)航系統(tǒng)姿態(tài)解算的算法有很多,其中最具有代表性的是其估計(jì)結(jié)果具有無(wú)偏、一致最優(yōu)的卡爾曼濾波(Kalman Filter, KF)及其擴(kuò)展算法:擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF),無(wú)跡卡爾曼濾波(Unscented Kalman Filter, UKF)及容積卡爾曼濾波(Cubature Kalman filter, CKF)[3]等,均廣泛應(yīng)用于航空航天,船舶和汽車的自主導(dǎo)航或輔助導(dǎo)航等方面。
KF濾波及其擴(kuò)展算法的無(wú)偏估計(jì)、一致最優(yōu)是在假設(shè)量測(cè)噪聲嚴(yán)格服從高斯分布的條件下成立的,但實(shí)際應(yīng)用中由于存在各種干擾情況,常常導(dǎo)致假設(shè)不成立,使估計(jì)結(jié)果達(dá)不到一致最優(yōu)。為消除這些干擾,文獻(xiàn)[4]針對(duì)目標(biāo)運(yùn)動(dòng)時(shí)出現(xiàn)理論模型和實(shí)際模型不匹配、模型失配的問(wèn)題,在傳統(tǒng)漸消濾波的基礎(chǔ)上提出了一種多重漸消因子的強(qiáng)跟蹤C(jī)KF算法。郭士犖等[5]在多重漸消因子的強(qiáng)跟蹤C(jī)KF算法的基礎(chǔ)上引入χ2檢驗(yàn)條件,使多重漸消因子的引入時(shí)機(jī)更加合理。徐博等[6]針對(duì)水下復(fù)雜環(huán)境,設(shè)計(jì)了一種基于馬氏距離的自適應(yīng)CKF算法,使用濾波新息序列的馬氏距離判斷系統(tǒng)模型失準(zhǔn)情況。文獻(xiàn)[7]則針對(duì)系統(tǒng)狀態(tài)躍變以及非高斯量測(cè)噪聲干擾等問(wèn)題,提出了一種基于m估計(jì)的魯棒卡爾曼濾波器,根據(jù)Huber的m估計(jì)方法重新定義了新息序列,使濾波的魯棒性得到提升。文獻(xiàn)[8]則借鑒Huber等價(jià)權(quán)函數(shù)的思想,構(gòu)造了逼近函數(shù)以抑制觀測(cè)野值的影響,提高了算法的魯棒性。但上述這些算法在處理濾波異常情況時(shí),是獨(dú)立考慮算法的自適應(yīng)性和魯棒性,而所針對(duì)的也是僅單一異常情況。然而實(shí)際慣導(dǎo)系統(tǒng)的算法在不同環(huán)境下運(yùn)行時(shí),需要考慮多種干擾混合共存的情況,同時(shí)考慮算法的自適應(yīng)性和魯棒性。
基于此,本文提出了一種模糊魯棒自適應(yīng)容積卡爾曼濾波(Fuzzy Robust Adaptive - Cubature Kalman Filter, FRA-CKF)算法,借鑒濾波新息序列的2χ檢驗(yàn),根據(jù)2χ分布上分位點(diǎn)與假設(shè)檢驗(yàn)原理設(shè)置了不同異常的修正門(mén)限,構(gòu)造了模糊修正邊界并提出了相應(yīng)的模糊修正準(zhǔn)則。將算法的自適應(yīng)性和魯棒性進(jìn)行了有機(jī)融合,提高了濾波性能。實(shí)驗(yàn)結(jié)果表明,本文所提出的算法在受到不同干擾時(shí)能夠及時(shí)對(duì)異常進(jìn)行修正,準(zhǔn)確地估計(jì)系統(tǒng)姿態(tài)。
本文以IMU系統(tǒng)中的陀螺儀為對(duì)象,用四元數(shù)方式描述姿態(tài)參數(shù),建立IMU系統(tǒng)的狀態(tài)方程。令四元數(shù)q為系統(tǒng)狀態(tài),則根據(jù)陀螺儀的輸出角速度和四元數(shù)的微分方程有:
其中 q=[q0,q1, q2,q3]T且為單位四元數(shù),Ω(ω)是由陀螺儀測(cè)量角速度構(gòu)成的矩陣,其矩陣形式為:
將式(1)進(jìn)行離散化處理,以方便迭代計(jì)算:
其中 qk=[ q0,k, q1,k, q2,k, q3,k]T為k時(shí)刻的四元數(shù)系統(tǒng)狀態(tài)。由于陀螺儀在測(cè)量過(guò)程中會(huì)受噪聲和自身漂移影響,所以陀螺儀實(shí)際輸出的角速度應(yīng)包含這部分誤差。設(shè)陀螺儀所受干擾引起的角速度影響為 ωΔ ,則實(shí)際輸出的角速度應(yīng)為: ωs= ωt+Δω,其中 ωt為真實(shí)的角速度。將 ωs= ωt+Δω帶入式(3)有:
IMU的加速度計(jì)通過(guò)測(cè)量重力矢量能夠獲得載體的俯仰信息和橫滾信息,磁強(qiáng)計(jì)通過(guò)測(cè)量磁場(chǎng)矢量能夠得到載體的航向信息,所以本文選用加速度計(jì)和磁強(qiáng)計(jì)為系統(tǒng)的量測(cè),對(duì)陀螺儀漂移進(jìn)行修正。
加速度計(jì)的輸出模型為:
其中上標(biāo)b表示載體坐標(biāo)系(右前上),上標(biāo)n表示導(dǎo)航坐標(biāo)系(東北天);an=[ 0 0g]T為導(dǎo)航坐標(biāo)系下的重力加速度,為k時(shí)刻載體坐標(biāo)系下的加速度測(cè)量值,為k時(shí)刻從導(dǎo)航坐標(biāo)系(n系)到載體坐標(biāo)系(b系)的旋轉(zhuǎn)矩陣,加速度計(jì)噪聲面垂直,因此使用加速度計(jì)量測(cè)系統(tǒng)的俯仰角信息和由于重力平面始終和系統(tǒng)的航向平橫滾角信息,旋轉(zhuǎn)矩陣形式為:
將式(6)中第一項(xiàng)的計(jì)算結(jié)果拆分出四元數(shù)qk,得到k時(shí)刻的加速度計(jì)量測(cè)矩陣:
將式(8)帶入式(6),可得加速度計(jì)輸出模型的線性形式:
磁強(qiáng)計(jì)的輸出模型為:
其中 hn= [ B · cosδ 0 -B ·sinδ]T為地磁場(chǎng)強(qiáng)度B在導(dǎo)航坐標(biāo)系的投影,δ為磁偏角,磁強(qiáng)計(jì)噪聲Vh,k~N(0, Rh,k),且Wk,Va,k和Vh,k三種噪聲是相互獨(dú)立的。同理,將式(10)中第一項(xiàng)的計(jì)算結(jié)果拆分出四元數(shù)qk,可得矩陣:
其中 B1= B ·cosδ, B2= B ·sinδ。將式(11)帶入式(10),可得磁強(qiáng)計(jì)輸出模型的線性形式:
聯(lián)立式(5)(9)(12)得到IMU系統(tǒng)的狀態(tài)方程和量測(cè)方程。
1)漸消自適應(yīng)濾波
KF及其擴(kuò)展算法估計(jì)結(jié)果的無(wú)偏、最優(yōu)是在假設(shè)量測(cè)噪聲服從高斯白噪聲分布的前提下才成立的。系統(tǒng)量測(cè)值和量測(cè)預(yù)測(cè)值之間的殘差稱為新息kη,在此假設(shè)成立的前提下,濾波的新息序列kη服從均值為零,協(xié)方差理論值為的高斯序列。
當(dāng)量測(cè)受到非高斯噪聲干擾時(shí)假設(shè)不成立,濾波估計(jì)精度將下降,傳統(tǒng)方式是通過(guò)新息序列協(xié)方差的理論值和計(jì)算值進(jìn)行判斷:非高斯噪聲影響時(shí),新息序列協(xié)方差的實(shí)際計(jì)算值和理論值是不相等的,新息序列協(xié)方差估計(jì)值的計(jì)算式為:
漸消自適應(yīng)KF是一種抑制濾波新息異常的簡(jiǎn)單有效方法[4],如果新息序列為高斯序列,則濾波新息序列的馬氏距離應(yīng)服從卡方分布。根據(jù)假設(shè)檢驗(yàn)原理和 χ2檢驗(yàn)原理,選取以 χ2分布顯著性水平為α的上分位點(diǎn)作為濾波評(píng)判標(biāo)準(zhǔn)。例如,若量測(cè)為三維時(shí),選取的上分位點(diǎn)= 11.345,通過(guò)查表得也就是說(shuō),在濾波正常的情況下,新息序列的馬氏距離大于的概率只有1%。根據(jù)假設(shè)檢驗(yàn)原理,若新息序列的馬氏距離大于則在99%的置信度下可以認(rèn)為濾波異常。將此判斷方式應(yīng)用到CKF中,則有:
其中dm為新息序列的馬氏距離。
在正常噪聲情況下 dm<,按照CKF濾波對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì);當(dāng) dm>時(shí),對(duì)CKF算法進(jìn)行修正,通過(guò)自適應(yīng)的方式調(diào)節(jié)這一現(xiàn)象:引入一個(gè)時(shí)變的標(biāo)量因子 kλ,僅針對(duì)該時(shí)刻的新息序列進(jìn)行估計(jì),對(duì)狀態(tài)預(yù)測(cè)誤差協(xié)方差Pk/k-1的第一項(xiàng)進(jìn)行調(diào)整:
通過(guò)高斯牛頓法進(jìn)行迭代求解,可以得到λk的遞推式[12]:
2)膨脹魯棒濾波
當(dāng)外部的測(cè)量信息不可靠時(shí),通過(guò)提高量測(cè)的權(quán)重從而抑制模型失配帶來(lái)的誤差,當(dāng)量測(cè)含有較大噪聲時(shí),若繼續(xù)使用漸消自適應(yīng)的方式進(jìn)行調(diào)整,則會(huì)影響濾波估計(jì)的精度。文獻(xiàn)[9]提出了一種與漸消自適應(yīng)濾波類似的方法,在KF的基礎(chǔ)上引入一個(gè)時(shí)變的標(biāo)量膨脹因子對(duì)新息協(xié)方差的第二項(xiàng)進(jìn)行調(diào)整,對(duì)量測(cè)噪聲進(jìn)行膨脹,削弱量測(cè)對(duì)濾波的修正作用。
當(dāng) dm>時(shí),同樣以CKF濾波為對(duì)象,對(duì)新息序列協(xié)方差中的量測(cè)噪聲協(xié)方差R進(jìn)行處理:
膨脹因子 μk也需要滿足:
同樣使用高斯牛頓法進(jìn)行迭代求解,得到膨脹因子 μk的遞推式[10]:
其中膨脹因子 μk的初值為1。
與漸消自適應(yīng)濾波相反,膨脹魯棒濾波通過(guò)對(duì)量測(cè)噪聲的膨脹作用,減小了濾波增益,削弱了量測(cè)的權(quán)重,從而提高系統(tǒng)自身的魯棒性。
這兩種方法分別針對(duì)不同異常情況提出了各自的調(diào)整方式,自適應(yīng)濾波更加相信量測(cè)信息的修正作用,而魯棒濾波則更看重系統(tǒng)模型的預(yù)測(cè)信息。兩種調(diào)節(jié)濾波的方式,就像“杠桿”一樣,是互相對(duì)立的調(diào)節(jié)方式,同一時(shí)刻在濾波算法中難以實(shí)現(xiàn)平衡[11]。在實(shí)際應(yīng)用中,系統(tǒng)不可避免地會(huì)出現(xiàn)狀態(tài)不連續(xù)變化時(shí)的模型失準(zhǔn),以及受外界干擾影響使量測(cè)含有非高斯噪聲的情況[12]。
因此,針對(duì)此情況提出了一種模糊修正準(zhǔn)則,將CKF、自適應(yīng)修正和魯棒修正相互貫通,將這根“杠桿”轉(zhuǎn)變?yōu)椤皹蛄骸?,使算法兼顧魯棒性和自適應(yīng)性。
新息序列的統(tǒng)計(jì)特性是判斷濾波是否異常的關(guān)鍵[9]。本文同樣利用2χ檢驗(yàn)原理對(duì)濾波異常判斷。并將設(shè)置為修正起始門(mén)限,以二維平面為例展示修正域,設(shè)原點(diǎn)為圓心,以修正門(mén)限為半徑作圓如圖1所示(參數(shù)β>0)。若k時(shí)刻沒(méi)有出現(xiàn)異常,新息序列的馬氏距離應(yīng)在小圓內(nèi),此時(shí)無(wú)需修正;若濾波出現(xiàn)異常,新息序列的馬氏距離超出修正半徑,此時(shí)需要根據(jù)模糊修正準(zhǔn)則執(zhí)行相應(yīng)的修正。
圖1 修正門(mén)限圓Fig.1 Modifiedthreshold circle
根據(jù)引起濾波異常的原因,需要使用不同方法進(jìn)行修正,但是如果直接設(shè)置自適應(yīng)修正的邊界和魯棒修正的邊界,嚴(yán)格按照界內(nèi)界外進(jìn)行修正,顯然是不合理的。如果dm出現(xiàn)在自適應(yīng)修正邊界內(nèi)但距離魯棒修正邊界很近的情況,或是在魯棒修正邊界內(nèi)但距離自適應(yīng)修正邊界很近的情況,嚴(yán)格按照界內(nèi)界外的標(biāo)準(zhǔn)可能會(huì)引起錯(cuò)誤修正,出現(xiàn)相反的修正結(jié)果。
為避免這種現(xiàn)象發(fā)生,合理修正異常情況,本文提出了模糊修正準(zhǔn)則,通過(guò)將修正邊界模糊化把邊界轉(zhuǎn)化為域,在域內(nèi)通過(guò)設(shè)置不同修正方式的隸屬度函數(shù),用隸屬程度這一概念將不修正、自適應(yīng)修正和魯棒修正三種修正方式融合,根據(jù)模糊修正準(zhǔn)則對(duì)濾波異常進(jìn)行合理修正。模糊修正準(zhǔn)則具體由三部分組成:
1) 設(shè)置模糊邊界
在設(shè)置不同異常的模糊邊界時(shí),考慮到系統(tǒng)狀態(tài)出現(xiàn)躍變導(dǎo)致模型失準(zhǔn)情況時(shí),實(shí)際量測(cè)值和量測(cè)預(yù)測(cè)值之間的差異較大,濾波新息會(huì)大幅度變化;而系統(tǒng)受到噪聲干擾或累積誤差引起的新息變化相對(duì)較小。因此,在修正門(mén)限的基礎(chǔ)上選定4個(gè)參數(shù)a,b,c,d分別作為CKF、自適應(yīng)修正和魯棒修正邊界,其中
2) 構(gòu)造隸屬度函數(shù)
三種算法標(biāo)準(zhǔn)CKF、魯棒修正(RCKF)和自適應(yīng)修正(ACKF)的隸屬度函數(shù)分別為:
三種隸屬度函數(shù)的選取原因是:當(dāng)新息序列的馬氏距離超過(guò)修正門(mén)限且不斷增大時(shí),不修正的隸屬度應(yīng)快速下降,所以選取了拋物線型為不修正的隸屬度函數(shù);此時(shí)魯棒修正的隸屬度應(yīng)逐漸增大,因此選取了增長(zhǎng)趨勢(shì)為線性的半梯形隸屬度函數(shù)。當(dāng)新息序列的馬氏距離增長(zhǎng)過(guò)魯棒修正邊界時(shí),魯棒修正的隸屬度也應(yīng)逐漸下降,若不選擇梯形而選擇拋物線型,則魯棒修正的程度會(huì)下降得過(guò)快從而使修正不到位。自適應(yīng)修正的隸屬度函數(shù)應(yīng)先緩慢增長(zhǎng)再快速上升,因此自適應(yīng)修正的隸屬度函數(shù)選擇半拋物線型。以a=2,b= 4,c=6,d=8為例,三種修正的隸屬度函數(shù)如圖2所示。
圖2 三種修正的隸屬度函數(shù)Fig.2 Membership functions of three modified
3) 制定模糊修正準(zhǔn)則
根據(jù)隸屬度函數(shù)對(duì)濾波異常進(jìn)行判斷并修正:當(dāng)dm在模糊域(0,a]之間時(shí),濾波無(wú)異常,執(zhí)行標(biāo)準(zhǔn)CKF算法。當(dāng)dm在模糊域(a,b)之間時(shí),濾波異常,執(zhí)行修正,此時(shí)既執(zhí)行CKF也執(zhí)行魯棒修正,但是隸屬于CKF的“程度”逐漸減小,隸屬于魯棒修正的“程度”逐漸增大。當(dāng)dm在模糊域[b,c)之間時(shí),新息異常是濾波受到噪聲干擾引起的,應(yīng)對(duì)量測(cè)噪聲做出調(diào)整降低量測(cè)的修正作用,因此執(zhí)行魯棒修正。當(dāng)dm在模糊域[c,d)之間時(shí),此時(shí)既執(zhí)行魯棒修正也執(zhí)行自適應(yīng)修正,但是隸屬于魯棒修正的“程度”逐漸減小,隸屬于自適應(yīng)修正的“程度”逐漸增大。當(dāng)dm超過(guò)模糊修正邊界d時(shí),新息異常是由狀態(tài)躍變或模型失配引起的,應(yīng)對(duì)系統(tǒng)預(yù)測(cè)協(xié)方差進(jìn)行調(diào)整,降低系統(tǒng)預(yù)測(cè)部分的權(quán)重,增大量測(cè)的修正作用,因此執(zhí)行自適應(yīng)修正。針對(duì)濾波異常時(shí)的模糊修正準(zhǔn)則如表1所示。整體流程如圖3所示。
表1 模糊修正準(zhǔn)則Tab.1 Fuzzy correction criterion
圖3 濾波異常判斷和模糊修正準(zhǔn)則Fig.3 Filter anomaly judgment and fuzzy correction criteri
為了驗(yàn)證所提模糊修正準(zhǔn)則在不同異常情況下對(duì)CKF算法的修正能力。本節(jié)在Matlab2018a環(huán)境下進(jìn)行仿真試驗(yàn)驗(yàn)證,為了接近本文所用IMU系統(tǒng)的模型,設(shè)置系統(tǒng)的狀態(tài)方程為:
式中:k =1,2,3…n為迭代次數(shù),x為系統(tǒng)狀態(tài)用四元數(shù)形式表示且初始值x(0)=[0,0,0,0]T,F(xiàn)為主對(duì)角線元素為1,其余元素為0.01的四階矩陣,I為四階單位矩陣,系統(tǒng)噪聲協(xié)方差和量測(cè)噪聲協(xié)方差分別為 Q = 10-4·I和 R = 10-1·I。量測(cè)噪聲在迭代的第40次時(shí)以指數(shù)形式擴(kuò)大一倍后不變,仿真了系統(tǒng)在不同環(huán)境不同場(chǎng)合運(yùn)行時(shí)的非高斯噪聲影響。在迭代至第40、41次時(shí),給系統(tǒng)狀態(tài)施加一個(gè)強(qiáng)度為50的正向脈沖信號(hào);在迭代至第80、81次時(shí)給系統(tǒng)狀態(tài)施加一個(gè)強(qiáng)度為150的反向脈沖信號(hào);在迭代至第120-125時(shí)給系統(tǒng)施加強(qiáng)度為20的反向階躍信號(hào),使系統(tǒng)狀態(tài)發(fā)生不連續(xù)躍變,仿真了系統(tǒng)運(yùn)動(dòng)狀態(tài)發(fā)生變化導(dǎo)致的模型失準(zhǔn)狀況。仿真結(jié)果如圖4所示(僅展示q0的仿真結(jié)果,其余元素與q0結(jié)果類似)。
圖4 q0的迭代結(jié)果Fig.4 Iteration result of q0
圖4 中本文所提的FRA-CKF算法有效地跟蹤了系統(tǒng)實(shí)際情況,在系統(tǒng)狀態(tài)發(fā)生躍變時(shí)做出了合理的自適應(yīng)修正,準(zhǔn)確地跟蹤系統(tǒng)狀態(tài),并且在僅受噪聲影響,未發(fā)生躍變時(shí),能夠進(jìn)行魯棒修正、漸消噪聲影響,有效地驗(yàn)證了所提模糊修正準(zhǔn)則。
為了進(jìn)一步驗(yàn)證模糊修正魯棒自適應(yīng)CKF算法準(zhǔn)確跟蹤系統(tǒng)狀態(tài)的能力,將該算法與漸消自適應(yīng)CKF算法(ACFK),魯棒CKF算法(RCFK)和CKF算法的估計(jì)結(jié)果進(jìn)行對(duì)比。將四種算法的濾波參數(shù)設(shè)置相同,對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì),結(jié)果如圖5所示。
圖5 不同濾波算法對(duì)比Fig.5 Comparison of different filtering algorithms
圖5 展示的濾波估計(jì)結(jié)果中能看出,所提FRA-CKF算法在系統(tǒng)狀態(tài)僅受噪聲干擾時(shí)的估計(jì)精度與魯棒CKF算法相當(dāng),高于CKF算法,自適應(yīng)CKF算法的估計(jì)精度最低;然而當(dāng)系統(tǒng)狀態(tài)發(fā)生躍變時(shí),魯棒CKF算法沒(méi)有跟蹤上系統(tǒng)的狀態(tài),CKF算法雖然勉強(qiáng)跟蹤上了系統(tǒng)的狀態(tài),但與實(shí)際系統(tǒng)狀態(tài)依然有較大的相差。只有自適應(yīng)CKF算法和FRA-CKF算法能夠跟蹤系統(tǒng)的變化,但自適應(yīng)CKF算法的估計(jì)精度受噪聲干擾,估計(jì)的結(jié)果振蕩不穩(wěn)定。只有FRA-CKF既能夠及時(shí)地跟蹤系統(tǒng)狀態(tài),也能在噪聲干擾下具有較高的精度和穩(wěn)定的估計(jì)結(jié)果。
為驗(yàn)證所提FRA-CKF算法在慣性導(dǎo)航姿態(tài)估計(jì)中的實(shí)際應(yīng)用效果,利用MEMS-IMU JY901進(jìn)行實(shí)測(cè)實(shí)驗(yàn)驗(yàn)證。
1)靜態(tài)實(shí)驗(yàn):將JY901接通好固定在無(wú)磁雙軸轉(zhuǎn)臺(tái)上,使芯片x軸對(duì)準(zhǔn)北向。轉(zhuǎn)臺(tái)開(kāi)機(jī)后轉(zhuǎn)置水平,在靜置1小時(shí)之后開(kāi)始采集數(shù)據(jù),采樣頻率為60 Hz,采樣時(shí)間為73 s,如圖6所示。
圖6 雙軸轉(zhuǎn)臺(tái)和帶有MEMS-IMU的測(cè)量載體Fig.6 Two axis turntable and carrier with MEMS-IMU
設(shè)置對(duì)比實(shí)驗(yàn):由于漸消自適應(yīng)CKF和魯棒CKF在濾波無(wú)異常時(shí)不執(zhí)行修正,與CKF算法為同一算法,而FRA-CKF算法通過(guò)設(shè)置修正邊界和模糊修正準(zhǔn)則在靜態(tài)條件下依然具有修正作用。所以靜態(tài)實(shí)驗(yàn)僅與CKF算法進(jìn)行對(duì)比。分別使用兩種算法對(duì)系統(tǒng)進(jìn)行姿態(tài)估計(jì),兩種算法的姿態(tài)估計(jì)結(jié)果如圖7-9所示。
圖7 俯仰角估計(jì)結(jié)果Fig.7 Pitch angle estimation results
圖8 橫滾角估計(jì)結(jié)果Fig.8 Roll angle estimation results
圖9 航向角估計(jì)結(jié)果Fig.9 Heading angle estimation results
從圖7-9中可以看出,在靜態(tài)條件下FRA-CKF算法相比CKF算法對(duì)姿態(tài)參數(shù)的估計(jì)精度更高,且波動(dòng)范圍小,具有穩(wěn)定估計(jì)結(jié)果。為了進(jìn)一步衡量?jī)煞N算法,表2給出了兩算法的均方根誤差。
表2 兩種算法的均方根誤差Tab.2 Root mean square error of two algorithms
從表2中能看出FRA-CKF算法相比CKF算法,靜態(tài)時(shí)姿態(tài)估計(jì)結(jié)果的均方根誤差,俯仰角降低了24%,橫滾角降低9%,航向角降低了80%,驗(yàn)證了算法的準(zhǔn)確性。
2)動(dòng)態(tài)實(shí)驗(yàn):對(duì)帶有MEMS-IMU JY901的測(cè)量載體進(jìn)行車載實(shí)驗(yàn),將載體連接好后固定在汽車后座上,記錄汽車的運(yùn)動(dòng)姿態(tài),如圖10所示。采樣頻率為60 Hz,采樣時(shí)間為13 min。行駛軌跡如圖11所示。
圖10 動(dòng)態(tài)車載實(shí)驗(yàn)Fig.10 Dynamic vehicle experiment
圖11 行駛軌跡Fig.11 Driving track
設(shè)置對(duì)比實(shí)驗(yàn):由于動(dòng)態(tài)車載實(shí)驗(yàn)測(cè)量數(shù)據(jù)會(huì)受外界干擾從而影響CKF算法,使算法出現(xiàn)異常情況。漸消自適應(yīng)CKF和魯棒CKF在異常情況將執(zhí)行相應(yīng)的修正,因此動(dòng)態(tài)實(shí)驗(yàn)將FRA-CKF、漸消自適應(yīng)CKF以及魯棒CKF算法進(jìn)行對(duì)比,進(jìn)而驗(yàn)證算法的實(shí)用性能。分別使用三種算法對(duì)系統(tǒng)進(jìn)行姿態(tài)估計(jì),根據(jù)實(shí)測(cè)數(shù)據(jù)三種算法的姿態(tài)估計(jì)結(jié)果如圖12-14所示。
圖12 俯仰角估計(jì)結(jié)果Fig.12 Pitch angle estimation results
圖13 橫滾角估計(jì)結(jié)果Fig.13 Roll angle estimation results
圖14 航向角估計(jì)結(jié)果Fig.14 Heading angle estimation results
圖12-14中的姿態(tài)信息表明:漸消自適應(yīng)CKF的俯仰信息和橫滾信息隨著航向信息的變化受到了較大干擾,而魯棒CKF不隨航向信息的變化而變化,始終穩(wěn)定。但兩種算法解算的航向信息在多次變化后的解算精度嚴(yán)重下降,第630 s后解算的航向嚴(yán)重偏離實(shí)際運(yùn)動(dòng)情況。而FRA-CKF算法在航向多次發(fā)生變化后仍能準(zhǔn)確地估計(jì),且俯仰信息和橫滾信息不敏感外界干擾,驗(yàn)證了算法兼顧自適應(yīng)與魯棒性的特點(diǎn)和在實(shí)際應(yīng)用中的有效性。
針對(duì)慣性導(dǎo)航系統(tǒng)受到狀態(tài)突變、未知量測(cè)噪聲等干擾,傳統(tǒng)濾波算法因發(fā)散而無(wú)法準(zhǔn)確估計(jì)系統(tǒng)姿態(tài)的問(wèn)題,提出了一種模糊魯棒自適應(yīng)容積卡爾曼濾波算法。通過(guò)設(shè)置起始修正門(mén)限對(duì)濾波新息的異常進(jìn)行判斷,并根據(jù)誤差來(lái)源設(shè)置了修正邊界,構(gòu)造隸屬度函數(shù)提出模糊修正準(zhǔn)則。根據(jù)模糊修正準(zhǔn)則對(duì)狀態(tài)預(yù)測(cè)協(xié)方差或量測(cè)噪聲協(xié)方差修正,使算法兼顧魯棒性和自適應(yīng)性。實(shí)驗(yàn)結(jié)果表明,該算法在靜態(tài)和動(dòng)態(tài)條件下相比其他算法具有準(zhǔn)確、穩(wěn)定的姿態(tài)估計(jì)結(jié)果。