加速度計和陀螺儀的實時降噪系統(tǒng)和方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及加速度計和巧螺儀降噪，尤其設(shè)及一種加速度計和巧螺儀的實時降噪 系統(tǒng)和方法。
【背景技術(shù)】
[0002] 慣性測量單元被廣泛的使用在慣性導(dǎo)航系統(tǒng)中，其中，慣性測量單元的零偏穩(wěn)定 性、常值誤差、標(biāo)度系數(shù)誤差、非線性誤差W及隨機(jī)噪聲對慣性導(dǎo)航系統(tǒng)的對準(zhǔn)精度、導(dǎo)航 解算精度都起著決定性作用。因此，提高慣性測量單元的量測精度和可靠性顯得至關(guān)重要。 提高慣性測量單元的量測精度的一種有效方法就是降低慣性測量單元的量測噪聲，提高信 噪比。
[0003] 早期，針對慣性測量單元降噪的最簡單的方法主要集中在低通濾波器的設(shè)計，但 低通濾波器只對固定帶寬的噪聲有很好的濾波效果，所W只能針對特定環(huán)境下的特定信息 進(jìn)行有效處理，而不適用于任意機(jī)動環(huán)境且低通濾波器存在時間延遲的問題較為嚴(yán)重。
[0004] 隨著計算機(jī)性能的提高，離散小波變換技術(shù)也被廣泛的應(yīng)用于慣性測量單元的降 噪處理，該方法在靜態(tài)環(huán)境中的運(yùn)用得到了很好的效果，但是在動態(tài)環(huán)境中該方法卻失效 且該方法也會引入時間延遲，降低信息的跟蹤能力。
[0005] 卡爾曼濾波可W有效處理系統(tǒng)過程隨機(jī)噪聲和量測隨機(jī)噪聲的能力，因此可W將 卡爾曼濾波運(yùn)用于慣性測量單元的隨機(jī)噪聲處理?，F(xiàn)有的自適應(yīng)卡爾曼濾波降噪方法雖然 可W適應(yīng)機(jī)動變化但是計算量較大，在高采樣頻率的實時環(huán)境中使用存在一定的困難。

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

[0006] 為了解決在動、靜態(tài)環(huán)境中皆能有效濾除加速度計和巧螺儀隨機(jī)量測噪聲的問 題，本發(fā)明提供了一種加速度計和巧螺儀的實時降噪系統(tǒng)及實時降噪方法，W達(dá)到減少濾 波時延，降低計算量，提高加速度計和巧螺儀量測精度的效果。
[0007] 為了解決上述問題，本發(fā)明一方面提供了一種加速度計和巧螺儀的實時降噪系 統(tǒng)，包括：
[000引慣性測量單元，用于提取加速度計和巧螺儀的量測信息，所述量測信息為被測載 體的線加速度和角速度信息；
[0009] 信息處理單元，用于對所述量測信息進(jìn)行卡爾曼濾波并在線調(diào)整卡爾曼濾波器參 數(shù)得到降噪后的量測信息；
[0010] 導(dǎo)航計算機(jī)，用于利用所述降噪后的量測信息進(jìn)行導(dǎo)航解算，輸出所述被測載體 的導(dǎo)航信息。
[0011] 進(jìn)一步地，所述信息處理單元包括：
[0012] 建立方程模塊，用于建立關(guān)于所述加速度計和巧螺儀的單輸入單輸出卡爾曼濾波 狀態(tài)方程和觀測方程；
[0013] 迭代計算模塊，用于進(jìn)行卡爾曼濾波迭代計算，并根據(jù)殘差X 2檢測法得到檢測函 數(shù)值丫 k，并將所述檢測函數(shù)值丫 k與預(yù)先設(shè)定的闊值進(jìn)行比較得出所述被測載體的機(jī)動變 化情況；
[0014] 在線調(diào)整模塊，用于根據(jù)所述被測載體的機(jī)動變化情況調(diào)整卡爾曼濾波器的濾波 增益矩陣K的值和觀測噪聲方差矩陣R的值，并將調(diào)整后的值反饋給迭代計算模塊。
[0015] 另一方面，本發(fā)明還提供了一種加速度計和巧螺儀的實時降噪方法，包括如下步 驟：
[0016] 提取加速度計和巧螺儀的量測信息，所述量測信息為被測載體的線加速度和角速 度信息；
[0017] 對所述量測信息進(jìn)行卡爾曼濾波并在線調(diào)整卡爾曼濾波器參數(shù)得到降噪后的量 測信息；
[0018] 利用所述降噪后的量測信息進(jìn)行導(dǎo)航解算，輸出所述被測載體的導(dǎo)航信息。
[0019] 進(jìn)一步地，所述在線調(diào)整卡爾曼濾波器參數(shù)的具體步驟為：
[0020] S1、建立關(guān)于加速度計和巧螺儀的單輸入單輸出卡爾曼濾波狀態(tài)方程和觀測方 程；
[0021] S2、進(jìn)行卡爾曼濾波迭代計算，并根據(jù)殘差X 2檢測法得到檢測函數(shù)值丫 k，并將所 述檢測函數(shù)值丫 k與預(yù)先設(shè)定的闊值進(jìn)行比較得出所述被測載體的機(jī)動變化情況；
[0022] S3、根據(jù)所述機(jī)動變化情況在線調(diào)整卡爾曼濾波器的濾波增益矩陣K的值和觀測 噪聲方差矩陣R的值。
[0023] 為了較準(zhǔn)確地反映被測載體的實際運(yùn)動情況，所述步驟S1中建立關(guān)于加速度計 和巧螺儀的單輸入單輸出卡爾曼濾波狀態(tài)方程和觀測方程的具體步驟為：
[0024] 將擾動信息作為系統(tǒng)隨機(jī)噪聲，且在采樣周期內(nèi)設(shè)線加速度的變化率為0, W所述 被測載體的線速度、線加速度為狀態(tài)量建立所述加速度計的連續(xù)狀態(tài)方程，W所述被測載 體的線加速度和量測噪聲建立所述加速度計的連續(xù)觀測方程；
[0025] 將擾動信息作為系統(tǒng)隨機(jī)噪聲，且在采樣周期內(nèi)設(shè)角加速度的變化率為0, W所述 被測載體的角速度、角加速度為狀態(tài)量建立所述巧螺儀的連續(xù)狀態(tài)方程，W所述被測載體 的角速度和量測噪聲建立所述巧螺儀的連續(xù)觀測方程；
[0026] 分別對所述加速度計的連續(xù)狀態(tài)方程和連續(xù)觀測方程，所述巧螺儀的連續(xù)狀態(tài)方 程和連續(xù)觀測方程進(jìn)行離散化處理，得到所述加速度計的離散卡爾曼濾波狀態(tài)方程和觀測 方程W及所述巧螺儀的離散卡爾曼濾波狀態(tài)方程和觀測方程。
[0027] 為了更好地適應(yīng)被測載體的實際運(yùn)動情況，尤其是機(jī)動狀態(tài)突變的情況，所述步 驟S2中獲取所述被測載體的機(jī)動變化情況的具體步驟為：
[002引將所述檢測函數(shù)值丫k與所述闊值比較，若所述檢測函數(shù)值丫 k大于所述闊值，則 判決被所述被測載體處于快速機(jī)動變化狀態(tài)；
[0029] 若所述檢測函數(shù)值丫 k小于闊值，則判決所述被測載體處于機(jī)動變化緩慢或勻速 運(yùn)動或靜止的狀態(tài)。
[0030] 為了提高降噪性能，所述步驟S3中在線調(diào)整卡爾曼濾波的濾波增益矩陣K的值和 觀測噪聲方差矩陣R的值的具體步驟為：
[0031] 設(shè)置濾波初始值，包括；加速度計的觀測噪聲方差陣R。、過程噪聲方差陣Q。、卡爾 曼濾波的估計誤差方差陣P。和系統(tǒng)狀態(tài)X巧螺儀的觀測噪聲方差陣Rg、過程噪聲方差陣 Qg、卡爾曼濾波的估計誤差方差陣Pg和系統(tǒng)狀態(tài)X g;
[0032] 當(dāng)判決所述被測載體處于快速機(jī)動變化狀態(tài)時，對當(dāng)前迭代計算中的濾波增益矩 陣K乘W-個大于1的加權(quán)系數(shù)并保持所述加速度計的觀測噪聲方差陣氏和所述巧螺儀 的觀測噪聲方差陣Rg的值不變，將得到的矩陣值用于后續(xù)迭代計算；
[0033] 當(dāng)判決所述被測載體處于機(jī)動變化緩慢或靜止或勻速機(jī)動狀態(tài)時，對當(dāng)前迭代計 算中的所述加速度計的觀測噪聲方差陣氏和所述巧螺儀的觀測噪聲方差陣Rg的值分別乘 W-個遠(yuǎn)大于1的加權(quán)系數(shù)且保持濾波增益陣K的值不變，將得到的矩陣值用于后續(xù)迭代 計算。
[0034] 為了保證加速計和巧螺測量值的濾波結(jié)果能夠?qū)崟r跟隨載體的機(jī)動，需要適當(dāng)將 Rg和R。初始值設(shè)置的小一些，所述加速度計的濾波初始值和所述巧螺儀的濾波初始值分別 設(shè)置為：
[003引 Xg= [0 0 0] T，Pg= diag[10 10 10]，
[0036] Rg= 0. 00001，Q g= diag 巧 X 10 -8 3X 1〇-8]，
[0037] Xa= [0 0 0] T，Pa= diag[10 10 10]，
[003引 Ra= 0. 00001，Q a= diag[5X 10 -4 5X l0-4]。
[0039] 本發(fā)明與現(xiàn)有技術(shù)相比，包括如下優(yōu)點(diǎn)：
[0040] 1.本發(fā)明針對加速度計和巧螺儀的特性分別建立了單輸入單輸出的卡爾曼濾波 狀態(tài)方程和觀測方程，濾波系統(tǒng)將加速度計和巧螺儀的量測噪聲作為卡爾曼濾波系統(tǒng)的觀 測噪聲處理，利用卡爾曼濾波的工作特性，有效降低了加速度計和巧螺儀量測數(shù)據(jù)中的隨 機(jī)噪聲。
[0041] 2.本發(fā)明利用殘差X 2檢測法對載體機(jī)動變化情況進(jìn)行實時監(jiān)測，殘差X 2檢測 法是一種無法確定故障原因、但能實時確定一組量測信息有效性的檢測方法，常常被用于 突變型故障檢測，實現(xiàn)了對載體機(jī)動變化情況進(jìn)行實時監(jiān)測的功能。
[0042] 3.本發(fā)明根據(jù)機(jī)動情況在線調(diào)整卡爾曼濾波的濾波增益陣K的值和觀測噪聲方 差陣R陣的值。加速度計和巧螺儀一般運(yùn)用在動態(tài)環(huán)境中，為了保證加速計和巧螺量測值 經(jīng)過濾波后的結(jié)果能夠?qū)崟r跟隨載體的機(jī)動，需要適當(dāng)將Rg和R。的初始值設(shè)置的小一些。 當(dāng)系統(tǒng)被判為處于快速機(jī)動變化的階段時，為了使濾波結(jié)果能夠更快的跟蹤載體的機(jī)動， 可W保持Rg和R。不變，同時，在當(dāng)前卡爾曼濾波迭代計算的出的濾波增益陣K前乘W-個 大于1的加權(quán)系數(shù)，然后在后續(xù)迭代計算中使用加權(quán)W后的K陣；當(dāng)系統(tǒng)被判為處于緩慢機(jī) 動變化或者靜止或勻速機(jī)動的階段時，為了增加系統(tǒng)的降噪性能，可W直接利用當(dāng)前卡爾 曼濾波迭代計算出的濾波增益陣K，但需要在Rg和R。前分