專利名稱：一種基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法及裝置的制作方法
一種基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法及裝置技術(shù)領(lǐng)域
本發(fā)明適用于高實時性、高精度要求的的導(dǎo)航系統(tǒng)領(lǐng)域，尤其適用于一種利用低成本設(shè)備獲取高精度結(jié)果的基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法及裝置。
背景技術(shù)：
導(dǎo)航系統(tǒng)的主要作用是提供實時地位置、速率、姿態(tài)信息，并且可以使之按照預(yù)先設(shè)定的路徑行走。但是由于外界的諸多干擾使有用信號參雜很多噪聲，卡爾曼濾波可以解決此類問題?？柭鼮V波是一種遞歸估計，只要獲知上一時刻狀態(tài)的估計值和前狀態(tài)的觀測值就可以計算出當(dāng)前狀態(tài)的估計值，因此不需要記錄觀測或者估計的歷史信息?？柭鼮V波器將位置、速度、姿態(tài)角信息融合起來，并且通過預(yù)測與更新來濾除噪聲，從而使獲取的信息更準(zhǔn)確，使控制更加精確。
卡爾曼濾波技術(shù)的發(fā)明給數(shù)字信號處理帶來了一場意義重大的革命?？柭鼮V波算法雖然很成熟，但是目前算法大多只支持少數(shù)狀態(tài)的濾波處理。由于算法中頻繁用到矩陣運算，其中包括矩陣相乘，相加減以及求逆，如果按照普通的運算法則將會消耗極大內(nèi)存空間，并且執(zhí)行時間冗長，效率低。所以設(shè)計高效的算法是很有必要的。此基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法及裝置是對十一個狀態(tài)量進(jìn)行融合濾波，通過研究矩陣運算來提高算法效率，從而降低濾波周期，使之在高頻低周期無操作系統(tǒng)的導(dǎo)航硬件中順利可靠運行。這樣可以減少對導(dǎo)航系統(tǒng)硬件的要求，降低成本。發(fā)明內(nèi)容
本發(fā)明主要目的是公開一種基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法，對導(dǎo)航方法中濾波過程進(jìn)行分階優(yōu)化以及對矩陣運算進(jìn)行改進(jìn)。分析十一階參數(shù)之間的聯(lián)系與獨立性，將濾波系統(tǒng)拆分成兩個子系統(tǒng)即一個五階系統(tǒng)，一個六階系統(tǒng)。通過研究矩陣的特征，改進(jìn)通用的算法，減少計算量，節(jié)省存儲空間從而達(dá)到提高效率的目的。
本發(fā)明目的還在于公開實現(xiàn)基于上述方法的一種導(dǎo)航裝置。
本發(fā)明的基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法通過如下步驟實現(xiàn)
(1)采集數(shù)據(jù)由數(shù)據(jù)采集模塊獲取當(dāng)前狀態(tài)觀測值，包括角速度P、q、r ；線加速度 、 、 ；地磁通量Hx、Hy、Hz ；速度和位置信息x、y、ζ、u、ν、w ；
(2)預(yù)處理數(shù)據(jù)將步驟(1)獲得角速度、線加速度、地磁通量轉(zhuǎn)換成所需要的姿態(tài)角，再進(jìn)一步轉(zhuǎn)化為四元數(shù)；
(3)建立模型利用步驟(1)、(2)獲得數(shù)據(jù)，對復(fù)雜非線性導(dǎo)航系統(tǒng)進(jìn)行泰勒展開降階線性化，分別建立狀態(tài)模型、狀態(tài)轉(zhuǎn)移模型和觀測模型；
(4)擴(kuò)展卡爾曼濾波根據(jù)卡爾曼濾波基本方程，利用步驟C3)建立的狀態(tài)量模型和上次狀態(tài)的估計值來預(yù)測當(dāng)前狀態(tài)的估計值，作為對當(dāng)前狀態(tài)觀測值的修正；
(5)輸出數(shù)據(jù)輸出修正后的當(dāng)前狀態(tài)的估計值。
傳感器元件得到的原始信息是不能直接被賦值給濾波器，必須經(jīng)過一定的轉(zhuǎn)換得到所需的姿態(tài)角(橫滾角識、俯仰角9、偏航角ψ)。其中橫滾角與俯仰角是通過線加速度來轉(zhuǎn)換得到，偏航角是通過地磁通量來轉(zhuǎn)換 得到。在得到姿態(tài)角后需要通過轉(zhuǎn)換公式將姿態(tài)角轉(zhuǎn)化為四元數(shù)。轉(zhuǎn)換公式如下所述線加速度轉(zhuǎn)角度
權(quán)利要求
1.一種基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法，其特征在于包括以下步驟(1)采集數(shù)據(jù)由數(shù)據(jù)采集模塊獲取當(dāng)前狀態(tài)觀測值，包括角速度P、q、r；線加速度a,、a7、az ；地磁通量 Γρ Ιψ Hx ；速度和位置信息χ、y、ζ、ν, w ；(2)預(yù)處理數(shù)據(jù)將步驟(1)獲得角速度、線加速度、地磁通量轉(zhuǎn)換成所需要的姿態(tài)角， 再進(jìn)一步轉(zhuǎn)化為四元數(shù)；(3)建立模型利用步驟(1)、(2)獲得數(shù)據(jù)，對復(fù)雜非線性導(dǎo)航系統(tǒng)進(jìn)行泰勒展開降階線性化，分別建立狀態(tài)模型、狀態(tài)轉(zhuǎn)移模型和觀測模型；(4)擴(kuò)展卡爾曼濾波根據(jù)卡爾曼濾波基本方程，利用步驟(3)建立的模型和上次狀態(tài)的估計值來預(yù)測當(dāng)前狀態(tài)的估計值，作為對當(dāng)前狀態(tài)觀測值的修正；(5)輸出數(shù)據(jù)輸出修正后的當(dāng)前狀態(tài)的估計值。
2.根據(jù)權(quán)利要求1所述的基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法，其特征在于步驟(2)姿態(tài)角包括橫滾角-俯仰角沒和偏航角辦，由下式得到
3.根據(jù)權(quán)利要求1所述的基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法，其特征在于步驟(3)中所述狀態(tài)模型以位置、速度、四元數(shù)以及重力加速度為參數(shù)對當(dāng)前狀態(tài)進(jìn)行修正得到矩陣；所述是狀態(tài)轉(zhuǎn)移模型由狀態(tài)模型矩陣的元素分別對位置、速度、四元數(shù)以及重力加速度求偏導(dǎo)而得；觀測模型是由觀測量分別對四元數(shù)求偏導(dǎo)而得。
4.根據(jù)權(quán)利要求1所述的基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法，其特征在于步驟(4)擴(kuò)展卡爾曼濾波通過以下方式處理矩陣求逆運算判斷主元是否為零是，則采用高斯-約當(dāng)算法；否，則采用全選主元算法。
5.根據(jù)權(quán)利要求1所述的基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法，其特征在于步驟(4)擴(kuò)展卡爾曼濾波將狀態(tài)模型和觀測模型分為兩個子系統(tǒng)，對于具有不同特征的狀態(tài)量進(jìn)行分塊、按序濾波系統(tǒng)一將速度、位置變量既作為狀態(tài)向量也作為觀測向量；系統(tǒng)二將四元數(shù)作為狀態(tài)向量，將姿態(tài)角作為觀測向量。
6.根據(jù)權(quán)利要求1所述的基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法，如果已知矩陣運算結(jié)果為對稱矩陣，可以通過下面方式來對算法進(jìn)行簡化(1)初始化結(jié)果存放的矩陣，將其下(上)三角元素初始化為零；(2)在計算過程中只計算相應(yīng)的下(上)三角矩陣元素；(3)將下(上)三角矩陣中的元素對稱賦給上(下)三角矩陣。
7.一種基于擴(kuò)展卡爾曼濾波的導(dǎo)航裝置，包括數(shù)據(jù)采集模塊、數(shù)據(jù)處理系統(tǒng)和數(shù)據(jù)反饋模塊，所述數(shù)據(jù)采集模塊用于采集當(dāng)前狀態(tài)觀測值，所述信號處理系統(tǒng)用于將當(dāng)前狀態(tài)觀測值轉(zhuǎn)換為所需姿態(tài)角并進(jìn)一步轉(zhuǎn)換為所需模型矩陣，所述數(shù)據(jù)反饋模塊用于輸出當(dāng)前狀態(tài)估計值，其特征在于還包括擴(kuò)展卡爾曼濾波器，所述擴(kuò)展卡爾曼濾波器接收上述信號處理系統(tǒng)的信號，通過擴(kuò)展卡爾曼濾波，將當(dāng)前狀態(tài)觀測值修正為估計值，并反饋給數(shù)據(jù)反饋模塊，得到精確導(dǎo)航數(shù)據(jù)。
8.根據(jù)權(quán)利要求7所述的擴(kuò)展卡爾曼濾波導(dǎo)航裝置，其特征在于所述數(shù)據(jù)采集模塊包括電子羅盤、全球定位系統(tǒng)和慣性測量元件；所述電子羅盤用于測量地磁通量，所述全球定位系統(tǒng)用以測量速度和位置信息。
9.根據(jù)權(quán)利要求8所述的擴(kuò)展卡爾曼濾波導(dǎo)航裝置，其特征在于所述慣性測量元件包括加速度計和陀螺儀；所述加速度計用于測量線加速度，陀螺儀用于測量角速度。
10.根據(jù)權(quán)利要求9所述的擴(kuò)展卡爾曼濾波導(dǎo)航裝置，其特征在于所述擴(kuò)展卡爾曼濾波器和數(shù)據(jù)處理系統(tǒng)由ARM7芯片提供；所述ARM7芯片通過SPI接口與慣性導(dǎo)航模塊和電子羅盤通訊；所述ARM7芯片通過DEBUG 口與GPS模塊通訊。
全文摘要
本發(fā)明公開了一種基于擴(kuò)展卡爾曼濾波的導(dǎo)航方法及裝置。通過對實際導(dǎo)航系統(tǒng)建立模型，模型包括狀態(tài)模型、狀態(tài)轉(zhuǎn)移模型和觀測模型，對導(dǎo)航方法中濾波過程進(jìn)行分階優(yōu)化以及對矩陣運算進(jìn)行改進(jìn)。分析十一階參數(shù)之間的聯(lián)系與獨立性，對矩陣運算進(jìn)行研究、對系統(tǒng)進(jìn)行拆分處理。從而達(dá)到了運算周期短，減少計算量，節(jié)省存儲空間從而提高效率的目的,同時也降低了對硬件的要求。
文檔編號G01C21/20GK102519463SQ20111041691
公開日2012年6月27日 申請日期2011年12月13日 優(yōu)先權(quán)日2011年12月13日
發(fā)明者張鋮, 申文斌, 裴海龍 申請人:華南理工大學(xué)