專利名稱：一種基于擴展卡爾曼濾波的導(dǎo)航裝置的制作方法
技術(shù)領(lǐng)域：
本實用新型適用于高實時性、高精度要求的的導(dǎo)航系統(tǒng)領(lǐng)域，尤其適用于一種利用低成本設(shè)備獲取高精度結(jié)果的基于擴展卡爾曼濾波的導(dǎo)航裝置。
背景技術(shù)：
導(dǎo)航系統(tǒng)的主要作用是提供實時地位置、速率、姿態(tài)信息，并且可以使之按照預(yù)先設(shè)定的路徑行走。但是由于外界的諸多干擾使有用信號參雜很多噪聲，卡爾曼濾波可以解決此類問題?？柭鼮V波是一種遞歸估計，只要獲知上一時刻狀態(tài)的估計值和前狀態(tài)的觀測值就可以計算出當前狀態(tài)的估計值，因此不需要記錄觀測或者估計的歷史信息?？柭鼮V波器將位置、速度、姿態(tài)角信息融合起來，并且通過預(yù)測與更新來濾除噪聲，從而使獲取的信息更準確，使控制更加精確?？柭鼮V波技術(shù)的實用新型給數(shù)字信號處理帶來了一場意義重大的革命。卡爾曼 濾波算法雖然很成熟，但是目前算法大多只支持少數(shù)狀態(tài)的濾波處理。由于算法中頻繁用到矩陣運算，其中包括矩陣相乘，相加減以及求逆，如果按照普通的運算法則將會消耗極大內(nèi)存空間，并且執(zhí)行時間冗長，效率低。所以設(shè)計高效的算法是很有必要的。此基于擴展卡爾曼濾波的導(dǎo)航方法及裝置是對十一個狀態(tài)量進行融合濾波，通過研究矩陣運算來提高算法效率，從而降低濾波周期，使之在高頻低周期無操作系統(tǒng)的導(dǎo)航硬件中順利可靠運行。這樣可以減少對導(dǎo)航系統(tǒng)硬件的要求，降低成本。
發(fā)明內(nèi)容本實用新型主要目的是公開一種基于擴展卡爾曼濾波的導(dǎo)航裝置，減少系統(tǒng)計算量，節(jié)省存儲空間從而達到提聞效率的目的。一種基于擴展卡爾曼濾波的導(dǎo)航裝置，如圖I所示，包括數(shù)據(jù)采集模塊、數(shù)據(jù)處理系統(tǒng)和數(shù)據(jù)反饋模塊，所述數(shù)據(jù)采集模塊用于采集當前狀態(tài)觀測值，所述信號處理系統(tǒng)用于將當前狀態(tài)觀測值轉(zhuǎn)換為所需姿態(tài)角并進一步轉(zhuǎn)換為所需模型矩陣,所述數(shù)據(jù)反饋模塊用于輸出當前狀態(tài)估計值。所述導(dǎo)航裝置還包括擴展卡爾曼濾波器，所述擴展卡爾曼濾波器接收上述信號處理系統(tǒng)的信號，通過擴展卡爾曼濾波，將當前狀態(tài)觀測值修正為估計值，并反饋給數(shù)據(jù)反饋模塊，得到精確導(dǎo)航數(shù)據(jù)。所述數(shù)據(jù)采集模塊包括電子羅盤、全球定位系統(tǒng)和慣性測量元件；所述電子羅盤用于測量地磁通量，所述全球定位系統(tǒng)用以測量速度和位置信息。所述慣性測量元件包括加速度計和陀螺儀；所述加速度計用于測量線加速度，陀螺儀用于測量角速度。所述擴展卡爾曼濾波器和數(shù)據(jù)處理系統(tǒng)由ARM7芯片提供；所述ARM7芯片通過SPI接口與慣性導(dǎo)航模塊和電子羅盤通訊；所述ARM7芯片通過DEBUG 口與GPS模塊通訊。所述數(shù)據(jù)處理系統(tǒng)由算法實現(xiàn)，將慣性測量元件獲取的角速度和線加速度，通過公式(I)轉(zhuǎn)換為橫滾角和俯仰角。電子羅盤獲取的地磁通量通過公式(2)得到偏航角Ψο再通過公式(3)得到四元數(shù)。最后將全球定位系統(tǒng)獲得的位置、速度信息，與四元數(shù)和重力加速融合，得到狀態(tài)量。本實用新型基本原理如下所述，裝置運行步驟(I)采集數(shù)據(jù)由數(shù)據(jù)采集模塊獲取當前狀態(tài)觀測值，包括角速度P、q、r ;線加速度ax、ay、az ;地磁通量Hx、Hy> Hz ;速度和位置信息x、y、z、U、V、w ;(2)預(yù)處理數(shù)據(jù)將步驟(I)獲得角速度、線加速度、地磁通量轉(zhuǎn)換成所需要的姿態(tài)角，再進一步轉(zhuǎn)化為四元數(shù)；(3)建立模型利用步驟(I)、(2)獲得數(shù)據(jù)，對復(fù)雜非線性導(dǎo)航系統(tǒng)進行泰勒展開 降階線性化，分別建立狀態(tài)模型、狀態(tài)轉(zhuǎn)移模型和觀測模型；(4)擴展卡爾曼濾波根據(jù)卡爾曼濾波基本方程，利用步驟(3)建立的狀態(tài)量模型和上次狀態(tài)的估計值來預(yù)測當前狀態(tài)的估計值，作為對當前狀態(tài)觀測值的修正；(5)輸出數(shù)據(jù)輸出修正后的當前狀態(tài)的估計值。傳感器元件得到的原始信息是不能直接被賦值給濾波器，必須經(jīng)過一定的轉(zhuǎn)換得到所需的姿態(tài)角(橫滾角爐、俯仰角Θ、偏航角V)。其中橫滾角與俯仰角是通過線加速度來轉(zhuǎn)換得到，偏航角是通過地磁通量來轉(zhuǎn)換得到。在得到姿態(tài)角后需要通過轉(zhuǎn)換公式將姿態(tài)角轉(zhuǎn)化為四元數(shù)。轉(zhuǎn)換公式如下所述線加速度轉(zhuǎn)角度
_]g = ^2x + a2y + a2zΦ = a tan 2(ay，az)(I)Θ = a tan 2(ax，g)地磁通量轉(zhuǎn)角度Xh = Hx= Hxcos0 + Hysin6sir^ - zsinecos(j)
Λ,Λ,Yh = Hy = H ycos(j) + H zsin(j)(2)
(H λ(xh\ψ = arctan ~— = arctan -
VYh)角度轉(zhuǎn)四元數(shù)shphi = sin ^chphi = cos *shpsi = sin —chtheta = cos —
2 2shtheta = sin —chpsi = cos —
2 2
q0 chphi * chtheta * chpsi + shphi * shtheta * shpsi qx -chphi * shtheta * shpsi + shphi * chtheta * chpsi=
q2 chphi * shtheta * chpsi + shphi * chtheta * shpsi
(3)
chphi * chtheta * shpsi - shphi * shtheta * chpsi[0031]在實際工程中，大多數(shù)系統(tǒng)為非線性或系統(tǒng)噪聲非白噪聲，普通卡爾曼濾波器就不再是最優(yōu)，但系統(tǒng)的性能通常是可接受的次優(yōu)，采用泰勒級數(shù)舍項的線性化方法使非線性系統(tǒng)近似為線性系統(tǒng)。建立的模型主要包括狀態(tài)量模型、狀態(tài)轉(zhuǎn)移模型以及觀測模型假設(shè)非線性系統(tǒng)的模型是X = f{X(k), u(k\ k、Z (k) = h (X (k), k)這個非線性模型的狀態(tài)量就是運載體的位置、速度、四元數(shù)表示出的姿態(tài)以及重力加速度，即X = [x y z U V w q0 Q1 q2 q3 g]T觀測量就是位置、速度以及姿態(tài)角，SP
權(quán)利要求1.一種基于擴展卡爾曼濾波的導(dǎo)航裝置，包括數(shù)據(jù)采集模塊、數(shù)據(jù)處理系統(tǒng)和數(shù)據(jù)反饋模塊，所述數(shù)據(jù)采集模塊用于采集當前狀態(tài)觀測值，所述信號處理系統(tǒng)用于將當前狀態(tài)觀測值轉(zhuǎn)換為所需姿態(tài)角并進一步轉(zhuǎn)換為所需模型矩陣，所述數(shù)據(jù)反饋模塊用于輸出當前狀態(tài)估計值，其特征在于還包括擴展卡爾曼濾波器，所述擴展卡爾曼濾波器接收上述信號處理系統(tǒng)的信號，通過擴展卡爾曼濾波，將當前狀態(tài)觀測值修正為估計值，并反饋給數(shù)據(jù)反饋模塊，得到精確導(dǎo)航數(shù)據(jù)。
2.根據(jù)權(quán)利要求I所述的擴展卡爾曼濾波導(dǎo)航裝置，其特征在于所述數(shù)據(jù)采集模塊包括電子羅盤、全球定位系統(tǒng)和慣性測量元件。
3.根據(jù)權(quán)利要求2所述的擴展卡爾曼濾波導(dǎo)航裝置，其特征在于所述慣性測量元件包括加速度計和陀螺儀。
4.根據(jù)權(quán)利要求I所述的擴展卡爾曼濾波導(dǎo)航裝置，其特征在于所述擴展卡爾曼濾波器和數(shù)據(jù)處理系統(tǒng)由ARM7芯片提供；所述ARM7芯片通過SPI接口與慣性導(dǎo)航模塊和電子羅盤通訊；所述ARM7芯片通過DEBUG 口與GPS模塊通訊。
專利摘要本實用新型公開了一種基于擴展卡爾曼濾波的導(dǎo)航裝置包括數(shù)據(jù)采集模塊、數(shù)據(jù)處理系統(tǒng)和數(shù)據(jù)反饋模塊，所述數(shù)據(jù)采集模塊用于采集當前狀態(tài)觀測值，所述信號處理系統(tǒng)用于將當前狀態(tài)觀測值轉(zhuǎn)換為所需姿態(tài)角并進一步轉(zhuǎn)換為所需模型矩陣，所述數(shù)據(jù)反饋模塊用于輸出當前狀態(tài)估計值，其特征在于還包括擴展卡爾曼濾波器，所述擴展卡爾曼濾波器接收上述信號處理系統(tǒng)的信號，通過擴展卡爾曼濾波，將當前狀態(tài)觀測值修正為估計值，并反饋給數(shù)據(jù)反饋模塊，得到精確導(dǎo)航數(shù)據(jù)。分析十一階參數(shù)之間的聯(lián)系與獨立性，對矩陣運算進行研究、對系統(tǒng)進行拆分處理。從而達到了運算周期短，減少計算量，節(jié)省存儲空間從而提高效率的目的，同時也降低了對硬件的要求。
文檔編號G01C21/20GK202511801SQ20112052196
公開日2012年10月31日 申請日期2011年12月13日 優(yōu)先權(quán)日2011年12月13日
發(fā)明者張鋮, 申文斌, 裴海龍 申請人:華南理工大學(xué)