專利名稱:一種低成本陀螺儀去噪防零漂防失真的方法
技術領域:
本發(fā)明專利涉及一種低成本陀螺儀去噪防零漂防失真的方法。
背景技術:
陀螺儀是一種重要的慣性敏感器件,用于對敏感相對慣性空間角運動進行測量, 是導航、制導、穩(wěn)定、瞄準等系統(tǒng)的核心部件,廣泛應用于戰(zhàn)略戰(zhàn)術武器、航天器、飛行器、汽車安全、工業(yè)自動化以及消費電子等領域,陀螺的精度將直接影響到系統(tǒng)姿態(tài)測量的效果。 受到制造工藝和精度水平的限制,相比傳統(tǒng)制造工藝制造的慣性傳感器,陀螺儀輸出數(shù)據(jù)的隨機噪聲較大,因而在使用陀螺儀前必需建立合理的隨機噪聲模型,對陀螺儀的誤差進行估算與補償,以減少其對系統(tǒng)精度的影響。請參閱圖1至圖4,陀螺儀的輸出誤差,除去安裝誤差、刻度因子誤差等可以忽略的外,主要有三種隨機誤差、常值漂移、溫度影響,由圖1可見,陀螺儀輸出信號噪聲非常大,且野值較多,最大的野值達到正負3.5 ;由圖2可見,在溫度影響下的陀螺輸出數(shù)據(jù),在 30000點附近出現(xiàn)了零位漂移;由圖3可見,陀螺儀常值漂移(曲線b),積分后角度(曲線a) 持續(xù)增大;由圖4可見,由于加速度傳感器的動態(tài)性能不好,慣性測量系統(tǒng)突然加速時,acc 傳感(曲線c)會反向掉一下,從而使得融合角度(曲線d)也相反方向偏移,與真實角度相反,出現(xiàn)失真。由圖可知,常值漂移對陀螺儀誤差產(chǎn)生的貢獻較大,對它的處理通常在機器人或兩輪平衡電動車等智能裝置啟動但還沒有工作時,對采集的陀螺儀的漂移數(shù)據(jù)進行平均, 然后在應用時在觀測數(shù)據(jù)中將其減去;其次,就是由隨機誤差和溫度影響。通常低成本陀螺儀去噪防零漂是采用時間序列建立誤差模型的方法,然而該方法面臨存在模型誤差,運用于復雜高機動系統(tǒng)時噪聲大,容易漂移。
發(fā)明內容
本發(fā)明是針對上述背景技術存在的缺陷提供一種低成本陀螺儀去噪防零漂防失真的方法,該方法可實現(xiàn)降低噪聲,減少漂移,解決了融合失真問題。為實現(xiàn)上述目的,本發(fā)明公開了一種低成本陀螺儀去噪防零漂防失真的方法,其包括如下步驟
步驟一,將陀螺儀的輸出信號通過低通濾波器去噪; 步驟二,將低通濾波器輸出的數(shù)據(jù)通過時間窗實時校正陀螺零漂; 步驟三,將通過時間窗實時校正后的輸出數(shù)據(jù)輸入kalman濾波器,通過kalman濾波殘差判斷陀螺儀失真與否。進一步地,所述低通濾波器的當前輸出值等于當前實際輸出值與前一次輸出值的權值和。進一步地,所述當前實際輸出值與前一次輸出值的權值為0. 7與0. 3或0. 6與 0. 4。
進一步地,所述步驟一的濾波次數(shù)為2 5次。進一步地,所述步驟二采用零速實時檢測校正算法。 進一步地,所述步驟二為通過先進先出隊列算法對低通濾波器輸出的數(shù)據(jù)通過時間窗的方式求平均值,并將該平均值輸入kalman濾波器。綜上所述,本發(fā)明低成本陀螺儀去噪防零漂防失真的方法通過上述各步驟的配合,從而可實現(xiàn)降低噪聲,減少漂移,解決了融合失真問題。
圖1為陀螺儀靜態(tài)下輸出的數(shù)據(jù)圖。圖2為在溫度影響下的陀螺儀輸出的數(shù)據(jù)圖。圖3為陀螺儀靜態(tài)時常值漂移和積分后的角度變化圖。圖4為陀螺儀與加速傳感器融合時的輸出數(shù)據(jù)圖。圖5為kalman濾波殘差與傳感器融合關系圖。圖6為本發(fā)明采取先進先出隊列,取15個點為大小的時間窗進行算法改進后的曲線圖。圖7為陀螺儀輸出的噪聲在使用本發(fā)明去噪方法的前后對比圖。圖8為陀螺儀輸出的動態(tài)性能在使用本發(fā)明去噪方法的前后對比圖。圖9為陀螺儀漂移曲線在使用本發(fā)明去噪方法的前后對比圖。圖10為使用本發(fā)明后的融合角度與角速度傳感器的角速度的關系曲線圖。
具體實施例方式為能進一步了解本發(fā)明的特征、技術手段以及所達到的具體目的、功能,下面結合附圖與具體實施方式
對本發(fā)明作進一步詳細描述。請參閱圖5至圖10,本發(fā)明一種低成本陀螺儀去噪防零漂防失真的方法包括如下步驟步驟一,將陀螺儀的輸出信號通過低通濾波器進行3次去噪,所述低通濾波器的濾波算法為當前輸出值等于當前實際輸出值與前一次輸出值的權值和,其中,所述當前實際輸出值與前一次輸出值的權值為0. 7與0. 3。步驟二,將低通濾波器輸出的數(shù)據(jù)通過時間窗實時校正陀螺零漂,在本實施例中, 其為采用零速實時檢測校正算法,即在陀螺儀的速度為零時通過先進先出隊列算法(FIFO 算法)對低通濾波器輸出的數(shù)據(jù)通過時間窗的方式求平均值,并將該平均值作為特征值輸入kalman濾波器。步驟三,將通過時間窗實時校正后的輸出數(shù)據(jù)輸入kalman濾波器,通過kalman濾波殘差判斷陀螺儀失真與否。由于陀螺儀漂移的原始信號中包含漸進項和隨機分量,通過多項式擬合提出漸進項后得到Kalman濾波殘差I^k=Zk-HkXtlri,Kalman濾波殘差代表濾波器可得到的新觀測的附加信息,靜態(tài)特性與先驗信息不同的數(shù)據(jù)將在殘差向量中體現(xiàn)出來。 由于所述公式Ik=Zk-HkXk^為本領域技術人員所了解,在此不再贅述。殘差代表新觀測器的信息,與自適應有最大關聯(lián),能精確、迅速地體現(xiàn)觀測量與估計值的偏差大小。因此,基于殘差構造一個模糊算法,用了剔除異常的測量數(shù)據(jù),加權調整估計值,使得濾波器殘差保持零均值,達到最優(yōu)估計。依據(jù)濾波殘差的模值大小不同,直接模糊調整濾波方差,強化濾波算法對目標機動的噪聲適應能力。請參閱圖5至圖10,圖5中曲線f為加速度傳感器的輸出曲線,曲線e為kalman 濾波殘差值,曲線g為傳感器融合角度,由圖中可以看出,濾波殘差可反映傳感器融合角度真實與否。圖6是采取先進先出隊列(FIFO隊列),取15個點為大小的時間窗進行算法改進后的曲線圖,曲線k為加速度傳感器的輸出值曲線,曲線h為濾波殘差方差值的曲線,曲線 i為傳感器融合的曲線。圖6中可以明顯地看出,時間軸3000點以前,傳感器融合正常,曲線h平穩(wěn)(0附近),時間軸3500點,3800點,4200點左右,加速度傳感器由于系統(tǒng)抖動,輸出值不正常,曲線h很靈敏地探測到,實時性非常好,并且其幅值表明了傳感器不正常工作的程度。圖7顯示了陀螺儀輸出的噪聲在使用本發(fā)明去噪方法的前(曲線m)后(曲線η)對比,使用本發(fā)明去噪方法后,陀螺儀的輸出數(shù)據(jù)精度提高了一倍,且消除了野值,噪聲降低近50%。圖8中,曲線ρ為原始數(shù)據(jù),曲線q為使用本發(fā)明去噪方法后的輸出數(shù)據(jù),其表明大幅降噪的同時,陀螺儀動態(tài)性能良好。圖9中曲線s為零漂補償后的曲線圖,曲線r為零漂補償前的曲線圖,由圖可知使用本發(fā)明去噪方法漂移減少了 57%。圖10顯示,使用本發(fā)明去噪方法后融合角度在加速度傳感器動態(tài)性能不好時,完全不失真。綜上所述,本發(fā)明低成本陀螺儀去噪防零漂防失真的方法通過上述各步驟的配合,從而可實現(xiàn)降低噪聲,減少漂移,解決了融合失真問題。以上所述實施例僅表達了本發(fā)明的一種實施方式,其描述較為具體和詳細,但并不能因此而理解為對本發(fā)明范圍的限制。應當指出的是,對于本領域的普通技術人員來說, 在不脫離本發(fā)明構思的前提下,還可以做出若干變形和改進,這些都屬于本發(fā)明的保護范圍。因此,本發(fā)明的保護范圍應以所附權利要求為準。
權利要求
1.一種低成本陀螺儀去噪防零漂防失真的方法,其包括如下步驟步驟一,將陀螺儀的輸出信號通過低通濾波器去噪;步驟二,將低通濾波器輸出的數(shù)據(jù)通過時間窗實時校正陀螺零漂;步驟三,將通過時間窗實時校正后的輸出數(shù)據(jù)輸入kalman濾波器,通過kalman濾波殘差判斷陀螺儀失真與否。
2.根據(jù)權利要求1所述的低成本陀螺儀去噪防零漂防失真的方法,其特征在于所述低通濾波器的當前輸出值等于當前實際輸出值與前一次輸出值的權值和。
3.根據(jù)權利要求2所述的低成本陀螺儀去噪防零漂防失真的方法,其特征在于所述當前實際輸出值與前一次輸出值的權值為0. 7與0. 3或0. 6與0. 4。
4.根據(jù)權利要求1或2或3所述的低成本陀螺儀去噪防零漂防失真的方法,其特征在于所述步驟一的濾波次數(shù)為2 5次。
5.根據(jù)權利要求1或2或3所述的低成本陀螺儀去噪防零漂防失真的方法,其特征在于所述步驟二采用零速實時檢測校正算法。
6.根據(jù)權利要求1或2或3所述的低成本陀螺儀去噪防零漂防失真的方法,其特征在于所述步驟二為通過先進先出隊列算法對低通濾波器輸出的數(shù)據(jù)通過時間窗的方式求平均值,并將該平均值輸入kalman濾波器。
全文摘要
本發(fā)明公開了一種低成本陀螺儀去噪防零漂防失真的方法,其包括如下步驟步驟一,將陀螺儀的輸出信號通過低通濾波器去噪;步驟二,將低通濾波器輸出的數(shù)據(jù)通過時間窗實時校正陀螺零漂;步驟三,將通過時間窗實時校正后的輸出數(shù)據(jù)輸入kalman濾波器,通過kalman濾波殘差判斷陀螺儀失真與否。本發(fā)明低成本陀螺儀去噪防零漂防失真的方法通過上述各步驟的配合,從而可實現(xiàn)降低噪聲,減少漂移,解決了融合失真問題。
文檔編號G01C25/00GK102288197SQ20101061273
公開日2011年12月21日 申請日期2010年12月30日 優(yōu)先權日2010年12月30日
發(fā)明者李一鵬, 涂超, 郭蓋華 申請人:東莞易步機器人有限公司