一種雙頻彈性干擾下角速度濾波方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種角速度濾波方法，特別是涉及一種角速度擴(kuò)展卡爾曼濾波方法。
【背景技術(shù)】
[0002] 同陀螺器件相比，角加速度計(jì)具有體積小、成本降低空間大、無漂移、安裝限制 少的特點(diǎn)。角加速度計(jì)通常被用來組建無陀螺儀的慣性測量單元（gyro-freeinertial measurementunitGF-IMU)，在航天飛行器中具有很好的應(yīng)用前景。在一些工程實(shí)踐中將 飛行器作為剛體模型來處理，認(rèn)為角加速度計(jì)只受到測量噪聲的影響。實(shí)際上，飛行器是一 個(gè)彈性體，除了測量噪聲外還受到飛行器自身彈性振動(dòng)的影響。這種彈性振動(dòng)對(duì)角速度測 量有著很大的影響。針對(duì)該問題，在以往工程實(shí)踐中，將彈性振動(dòng)建模為不同頻率和幅度的 正弦信號(hào)，并且假定該干擾信號(hào)的頻率是已知且固定的；干擾信號(hào)的幅度和角速度信號(hào)幅 度的比例關(guān)系是已知的，使用傳遞函數(shù)式的數(shù)字濾波器對(duì)角速度信號(hào)的測量值進(jìn)行濾波。 這種方法存在兩個(gè)缺陷。第一，濾波器傳遞函數(shù)的形式和彈性干擾信號(hào)的頻率有關(guān)，在不同 的場合下，彈性干擾的頻率可能不同，導(dǎo)致濾波器需要重新設(shè)計(jì)。這使得該方法適用面比較 窄。第二，傳遞函數(shù)形式的濾波器雖然對(duì)彈性干擾和測量噪聲進(jìn)行了濾除，但是會(huì)產(chǎn)生相位 延遲，導(dǎo)致對(duì)角速度信號(hào)估計(jì)值不準(zhǔn)確。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明為了解決現(xiàn)有的傳遞函數(shù)形式濾波器在面臨不同頻率的彈性干擾時(shí)需要 重新設(shè)計(jì)的問題和進(jìn)行彈性干擾和測量噪聲濾除時(shí)產(chǎn)生相位延遲而導(dǎo)致的角速度信號(hào)估 計(jì)值不準(zhǔn)確的問題。
[0004] 一種雙頻彈性干擾下角速度濾波方法，包括下述步驟：
[0005] 步驟1、針對(duì)彈性干擾下角速度信號(hào)動(dòng)態(tài)過程進(jìn)行建模：
[0006] 飛行器的角速度動(dòng)態(tài)過程為
[0008] 其中，ω代表角速度，a代表角加速度，Μ為轉(zhuǎn)動(dòng)力矩，J為轉(zhuǎn)動(dòng)慣量；
[0009] 彈性干擾信號(hào)為兩個(gè)正弦信號(hào)疊加而成，設(shè)兩個(gè)正弦信號(hào)的頻率分別為％和 ω2,單位為rad/s;兩個(gè)正弦信號(hào)的相位分別為料和0，幅度分別為匕和b2;
[0010] 彈性干擾信號(hào)的形式如下
[0011]
[0012] 將彈性干擾信號(hào)中的Μsin(呌/ +仍）進(jìn)行處理得到彈性干擾狀態(tài)Vl、v2，將彈性干 擾信號(hào)中的(嘩+奶）進(jìn)行處理得到彈性干擾狀態(tài)v3、v4;
[0013] 將干擾頻率％和ω2當(dāng)做狀態(tài)進(jìn)行估計(jì)；設(shè)狀態(tài)為X=[χv1v2v3v4c0iω2] T，其中x為角速度信號(hào)，即x=ω;則角速度和干擾信號(hào)的狀態(tài)方程為
[0015] 其中，az角加速度的測量值；
[0016] 角速度的測量方程為
[0017] z=x+v1+v3+w (4)
[0018] 式中，z為角速度的測量值；w是零均值高斯白噪聲；
[0019] 測量矩陣為
[0020]Η= [1 1 0 1 0 0 0] (5)
[0021 ] 步驟2、設(shè)計(jì)狀態(tài)轉(zhuǎn)移矩陣：
[0022] 由狀態(tài)方程（3)描述的系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣為
[0033] 其中，Τ為測量周期；
[0034] 步驟3、設(shè)計(jì)Kalman濾波方程：
[0035] 角速度濾波器采用EKF(擴(kuò)展卡爾曼濾波器）設(shè)計(jì)，Kalman濾波器方程為
[0039] Pk+1,k+1= (I_Kk+1H)Pk+1,k (24)
[0040] 式中，是對(duì)k+1時(shí)刻狀態(tài)的估計(jì)值；i:u是對(duì)k+1時(shí)刻狀態(tài)的預(yù)報(bào)值；Kk 和Kk+1分別是k時(shí)刻和k+1時(shí)刻的濾波器增益；Zk+1為k+1時(shí)刻的測量值；Φk+1,Ak時(shí)刻到k+1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；pk+lik為k+1時(shí)刻狀態(tài)預(yù)報(bào)的協(xié)方差矩陣；PkidPPk+lik+1分別是k 時(shí)刻和k+1時(shí)刻狀態(tài)估計(jì)協(xié)方差矩陣；Q為過程噪聲矩陣；R為測量噪聲矩陣；I為7X7維 單位矩陣；
[0041] 步驟4、根據(jù)步驟3設(shè)計(jì)的Kalman濾波方程對(duì)含有雙頻彈性干擾下的角速度進(jìn)行 濾波處理。
[0042] 本發(fā)明具有以下有益效果：
[0043] 本發(fā)明設(shè)計(jì)了一種基于擴(kuò)展卡爾曼濾波的角速度測量信號(hào)的濾波方法，適用于兩 個(gè)任意低頻正弦彈性干擾下的角速度測量值的干擾和測量噪聲濾除問題；設(shè)計(jì)好的濾波器 不需要根據(jù)不同的彈性干擾頻率重新設(shè)計(jì)或者修改，可以適用于任意低頻彈性干擾信號(hào)， 使得本發(fā)明的濾波方法適用范圍較廣；而且本發(fā)明的濾波方法可以估計(jì)出彈性干擾信號(hào)的 頻率。同時(shí)本發(fā)明的濾波方法針對(duì)角速度信號(hào)的估計(jì)值基本上沒有相位滯后，使得估計(jì)結(jié) 果更加準(zhǔn)確，估計(jì)誤差標(biāo)準(zhǔn)差相對(duì)于測量誤差標(biāo)準(zhǔn)差減小了 70 %以上。
【附圖說明】
[0044] 圖1為實(shí)施例中角速度信號(hào)真值、測量值和估計(jì)值的對(duì)比圖；
[0045] 圖2為實(shí)施例中彈性干擾狀態(tài)Vl下的估計(jì)仿真結(jié)果；
[0046] 圖3為實(shí)施例中彈性干擾狀態(tài)v2下的估計(jì)仿真結(jié)果；
[0047] 圖4為實(shí)施例中彈性干擾狀態(tài)v3下的估計(jì)仿真結(jié)果；
[0048] 圖5為實(shí)施例中彈性干擾狀態(tài)v4下的估計(jì)仿真結(jié)果；
[0049] 圖6為實(shí)施例中對(duì)兩個(gè)彈性干擾信號(hào)頻率的估計(jì)仿真結(jié)果。
【具體實(shí)施方式】
【具體實(shí)施方式】 [0050] 一：
[0051] -種雙頻彈性干擾下角速度濾波方法，包括下述步驟：
[0052] 步驟1、針對(duì)彈性干擾下角速度信號(hào)動(dòng)態(tài)過程進(jìn)行建模：
[0053] 飛行器的角速度動(dòng)態(tài)過程為
[0055] 其中，ω代表角速度，a代表角加速度，Μ為轉(zhuǎn)動(dòng)力矩，J為轉(zhuǎn)動(dòng)慣量；
[0056] 彈性干擾信號(hào)為兩個(gè)正弦信號(hào)疊加而成，設(shè)兩個(gè)正弦信號(hào)的頻率分別為％和 ω2,單位為rad/s;兩個(gè)正弦信號(hào)的相位分別為fV和朽，幅度分別為匕和b2;
[0057] 彈性干擾信號(hào)的形式如下
[0058]
[0059] 將彈性干擾信號(hào)中的郵+ ?〇進(jìn)行處理得到彈性干擾狀態(tài)A、v2，將彈性干 擾信號(hào)中的hsin (>/ +的）進(jìn)行處理得到彈性干擾狀態(tài)v3、v4;
[0060] 將干擾頻率ω^Ρω2當(dāng)做狀態(tài)進(jìn)行估計(jì)；設(shè)狀態(tài)為X=[χv1v2v3v4c0iω2] T，其中x為角速度信號(hào)，即x=ω;則角速度和干擾信號(hào)的狀態(tài)方程為
[0062] 其中，az角加速度的測量值；
[0063] 角速度的測量方程為
[0064]z=x+v1+v3+w (4)
[0065] 式中，z為角速度的測量值；w是零均值高斯白噪聲；
[0066] 測量矩陣為
[0067]Η= [1 1 0 1 0 0 0] (5)
[0068] 步驟2、設(shè)計(jì)狀態(tài)轉(zhuǎn)移矩陣：
[0069] 由狀態(tài)方程（3)描述的系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣為

[0080] 其中，τ為測量周期；
[0081] 步驟3、設(shè)計(jì)Kalman濾波方程：
[0082] 角速度濾波器采用EKF(擴(kuò)展卡爾曼濾波器）設(shè)計(jì)，Kalman濾波器方程為
[0087] 式中，氧+_是對(duì)k+Ι時(shí)刻狀態(tài)的估計(jì)值；ii+u是對(duì)k+Ι時(shí)刻狀態(tài)的預(yù)報(bào)值；心和 Kk+1分別是k時(shí)刻和k+Ι時(shí)刻的濾波器增益；Zk+1為k+Ι時(shí)刻的測量值；Φk+1,Ak時(shí)刻到k+1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；Pk+lik為k+1時(shí)刻狀態(tài)預(yù)報(bào)的協(xié)方差矩陣；P^和Pk+lik+1分別是k 時(shí)刻和k