基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法
【專利摘要】本發(fā)明公開了基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，該方法包括以下步驟：首先建立無人直升機(jī)運(yùn)動(dòng)過程中大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差模型；在氣壓高度誤差模型和傳統(tǒng)慣性/大氣阻尼回路的基礎(chǔ)上，隨后建立包含大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差和慣性導(dǎo)航系統(tǒng)傳感器誤差的六狀態(tài)阻尼卡爾曼濾波器的狀態(tài)方程和量測(cè)方程；最后在無人直升機(jī)飛行過程中對(duì)高度誤差和天向速度誤差進(jìn)行慣性/大氣導(dǎo)航系統(tǒng)阻尼卡爾曼濾波，獲得阻尼卡爾曼濾波后的組合導(dǎo)航系統(tǒng)導(dǎo)航結(jié)果。本發(fā)明方法能夠在無人直升機(jī)飛行過程中實(shí)現(xiàn)對(duì)大氣高度測(cè)量誤差的跟蹤和補(bǔ)償，從而改善大氣輔助慣性導(dǎo)航高度通道中高度的精度，適合于工程應(yīng)用。
【專利說明】
基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及無人直升機(jī)基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，屬于 慣性/大氣導(dǎo)航系統(tǒng)高度測(cè)量技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 無人直升機(jī)是一種有動(dòng)力、操控靈活的無人駕駛飛行器，可實(shí)現(xiàn)垂直起降、空中懸 停、原地轉(zhuǎn)彎及任意的前飛、后飛和側(cè)飛等特殊飛行方式，并且可以加載多種機(jī)載設(shè)備，執(zhí) 行多種任務(wù)，實(shí)現(xiàn)反復(fù)長航時(shí)飛行。無人直升機(jī)能夠超低空飛行躲避敵方雷達(dá)的探測(cè)和跟 蹤，戰(zhàn)場(chǎng)適應(yīng)能力強(qiáng)，可以完成許多固定翼飛機(jī)無法完成的任務(wù)，彌補(bǔ)固定翼飛機(jī)在飛行范 圍和用途方面的不足，最大限度滿足多方面的軍事需求。
[0003] 飛行高度是無人直升機(jī)的重要飛行參數(shù)，是無人直升機(jī)實(shí)現(xiàn)正常安全駕駛、準(zhǔn)確 導(dǎo)航與控制、有效偵察和攻擊等任務(wù)不可或缺的信息。通過垂直方向上測(cè)得加速度的雙時(shí) 間積分，可以獲得的垂直位置(高度），但垂直加速度的雙重積分是不穩(wěn)定的，加速度偏差可 以導(dǎo)致計(jì)算高度呈指數(shù)增長，因此，慣性導(dǎo)航系統(tǒng)依賴的加速度測(cè)量值積分取得高度估計(jì) 是不穩(wěn)定的系統(tǒng)。為了提供更穩(wěn)定的慣性導(dǎo)航系統(tǒng)，氣壓高度計(jì)等外部引用被單獨(dú)或組合 使用，但氣壓高度計(jì)在低高度測(cè)量精度差，易受天氣變化和陣風(fēng)影響而造成高度測(cè)量不準(zhǔn)。
[0004] 目前基于經(jīng)典控制理論設(shè)計(jì)的慣性/大氣高度阻尼方法，是大氣高度和慣性高度 兩種信息取長補(bǔ)短，進(jìn)而得到高度誤差不發(fā)散且動(dòng)態(tài)性能較好的高度信息，但其本質(zhì)上無 法完全解決大氣隨機(jī)性誤差的影響，目前關(guān)于大氣高度的研究還基本上只是停留在二階阻 尼與三階阻尼的研究中，鮮有深入研究阻尼系統(tǒng)理論和改進(jìn)研究。因此研究基于大氣高度 輔助的慣性導(dǎo)航六狀態(tài)阻尼卡爾曼濾波方法，估計(jì)出大氣高度計(jì)中高度測(cè)量誤差，從而提 高大氣測(cè)量輸出的準(zhǔn)確性，將具有突出的應(yīng)用價(jià)值。
【發(fā)明內(nèi)容】
[0005] 本發(fā)明所要解決的技術(shù)問題是:提供基于大氣輔助的慣性高度通道阻尼卡爾曼濾 波方法，該方法能夠在無人直升機(jī)飛行過程中實(shí)現(xiàn)對(duì)大氣高度測(cè)量誤差的跟蹤和補(bǔ)償，滿 足無人直升機(jī)動(dòng)態(tài)飛行對(duì)導(dǎo)航系統(tǒng)的高精度要求。
[0006] 本發(fā)明為解決上述技術(shù)問題采用以下技術(shù)方案：
[0007] 基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，包括如下步驟：
[0008] 步驟1，建立無人直升機(jī)運(yùn)動(dòng)過程中大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差模型，所述大氣數(shù) 據(jù)系統(tǒng)的高度誤差為氣壓高度表的標(biāo)度因素誤差、氣壓高度表偏置誤差和壓力偏置誤差；
[0009] 步驟2,在步驟1對(duì)無人直升機(jī)運(yùn)動(dòng)過程中大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差建模的基礎(chǔ) 上，將慣性導(dǎo)航系統(tǒng)高度通道的三個(gè)誤差量和大氣數(shù)據(jù)系統(tǒng)的三個(gè)高度誤差量作為系統(tǒng)的 狀態(tài)量，構(gòu)建無人直升機(jī)六狀態(tài)阻尼卡爾曼濾波器的狀態(tài)方程和量測(cè)方程；
[0010] 步驟3,對(duì)步驟2建立的狀態(tài)方程和量測(cè)方程進(jìn)行離散化處理以及狀態(tài)量、量測(cè)量 的更新，實(shí)現(xiàn)對(duì)大氣高度測(cè)量誤差的跟蹤和補(bǔ)償。
[0011]作為本發(fā)明的一種優(yōu)選方案，步驟1所述大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差模型為：
[001 2] Shads - Shb+hT3hs+3hp+UacJs+ 〇 ads，
[0013]其中，Shads為氣壓高度誤差、Shb為氣壓高度表的標(biāo)度因素誤差、hT為氣壓高度真 值、Shs為氣壓高度表偏置誤差、ShP為壓力偏置誤差、uads為氣壓高度觀測(cè)噪聲、co ads為大氣 高度白噪聲;Shb、Shs、ShP的數(shù)學(xué)表達(dá)式為：
[0015]
，h為無人直升機(jī)的真實(shí)高度，mi = 44.32km， m2 = 0 · 190263，m3 = 45 · 395km，，m4= 14.605km，為系統(tǒng)極點(diǎn)，ωΒ為測(cè)量誤 差噪聲。
[0016] 作為本發(fā)明的一種優(yōu)選方案，步驟2所述無人直升機(jī)六狀態(tài)阻尼卡爾曼濾波器的 狀態(tài)方程為：
[0017] X = FX + GW，
[0018] 其中，X為系統(tǒng)狀態(tài)變量、X為狀態(tài)變量X的一階導(dǎo)數(shù)、F為系統(tǒng)矩陣、G為系統(tǒng)噪聲 系數(shù)矩陣、W為系統(tǒng)噪聲矩陣，狀態(tài)方程表示為：
[0020]其中，δΚδνζ、δ&分別為慣性導(dǎo)航系統(tǒng)高度通道中的垂直位置誤差、垂直速度誤 差、垂直加速度誤差狀態(tài)量;3hb、3hs、5hP分別為大氣數(shù)據(jù)系統(tǒng)中的氣壓高度表的標(biāo)度因素 誤差、氣壓高度表偏置誤差、壓力偏置誤差狀態(tài)量;分別為大氣高度三階阻尼回路 比例參數(shù)
Λ為無人直升機(jī)的真實(shí)高度，mi = 44.32km，m2 = 0.190263,m3 = 45.395km,1114=14.605km
為系統(tǒng)極點(diǎn)，0?2為舒勒頻率，uads 為氣壓高度觀測(cè)噪聲JAZ為垂直加速度誤差，ωΒ為測(cè)量誤差噪聲，coads為大氣高度白噪聲。
[0021] 作為本發(fā)明的一種優(yōu)選方案，步驟2所述無人直升機(jī)六狀態(tài)阻尼卡爾曼濾波器的 量測(cè)方程為：
[0022] Z = HX+V,
[0023] 其中，Z為觀測(cè)量矩陣、Η為量測(cè)矩陣、X為系統(tǒng)狀態(tài)變量、V為量測(cè)噪聲矩陣;量測(cè)方 程表示為：
[0024] Z(t) = [hi-hA]=H(t)X(t)+V(t) = (l 0 -1 -h -l)X(t)+V(t)，
[0025] 其中，hi = ht+3h，hA=ht-3hP，H(t) = [l]ixi，V(t)為系統(tǒng)測(cè)量白噪聲矩陣，h為無人 直升機(jī)的真實(shí)高度，他為慣性導(dǎo)航系統(tǒng)高度通道中的垂直位置誤差，Sh P為壓力偏置誤差，t 為時(shí)間。
[0026] 作為本發(fā)明的一種優(yōu)選方案，所述步驟3的具體過程為：
[0027] 31)將濾波器狀態(tài)方程和量測(cè)方程離散化處理得到：
[0029] 其中，Xk為tk時(shí)刻系統(tǒng)狀態(tài)變量，Xk-i為tk-i時(shí)刻系統(tǒng)狀態(tài)變量，?k,k-i為tk-i時(shí)刻至 tk時(shí)刻系統(tǒng)的一步狀態(tài)轉(zhuǎn)移矩陣，Γ 為tk-i時(shí)刻至tk時(shí)刻系統(tǒng)噪聲系數(shù)矩陣，Wk-i為tk-i 時(shí)刻系統(tǒng)噪聲矩陣，Zk為tk時(shí)刻的位置、速度及姿態(tài)觀測(cè)量矩陣，Hk為tk時(shí)刻量測(cè)矩陣，Vk為 tk時(shí)刻量測(cè)噪聲矩陣；
[0030] 32)對(duì)離散化處理得到的公式加入控制項(xiàng)UH，并采用閉環(huán)修正系統(tǒng)狀態(tài)方程對(duì)慣 導(dǎo)系統(tǒng)誤差進(jìn)行修正：
[0032]其中，Bk,k-i為tk-i時(shí)刻至tk時(shí)刻系統(tǒng)的控制項(xiàng)系數(shù)矩陣；
[0033] 33)得到系統(tǒng)的線性化卡爾曼濾波器方程：
[0039] 其中，氧pStk-i時(shí)刻至tk時(shí)刻一步預(yù)測(cè)狀態(tài)量;時(shí)刻濾波狀態(tài)估計(jì)量； 尤為tk時(shí)刻濾波狀態(tài)估計(jì)量;Kk為tk時(shí)刻濾波增益矩陣;Zk為t k時(shí)刻的新息序列;Pk.k-Atw 時(shí)刻至tk時(shí)刻一步預(yù)測(cè)協(xié)方差矩陣；Pk-i為tk-i時(shí)刻濾波狀態(tài)估計(jì)協(xié)方差矩陣；?k,k-iTS 的轉(zhuǎn)置矩陣；時(shí)刻系統(tǒng)噪聲系數(shù)矩陣;Qk-iStk-i時(shí)刻的系統(tǒng)觀測(cè)噪聲估計(jì) 協(xié)方差陣；Γ卜/為Γ ^的轉(zhuǎn)置矩陣;/?為Hk的轉(zhuǎn)置矩陣;Rk為tk時(shí)刻的量測(cè)噪聲估計(jì)協(xié)方差 陣;Pk為tk時(shí)刻濾波狀態(tài)估計(jì)協(xié)方差矩陣;I為單位矩陣；
[0040] 34)根據(jù)33)得到的線性化卡爾曼濾波器方程估計(jì)系統(tǒng)高度誤差值，并用大氣數(shù)據(jù) 系統(tǒng)的氣壓高度值減去系統(tǒng)高度誤差值，得到大氣數(shù)據(jù)系統(tǒng)的氣壓高度修正值。
[0041] 本發(fā)明采用以上技術(shù)方案與現(xiàn)有技術(shù)相比，具有以下技術(shù)效果：
[0042] 本發(fā)明基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，該方法通過將3個(gè)慣 性導(dǎo)航系統(tǒng)誤差狀態(tài)量和3個(gè)大氣數(shù)據(jù)系統(tǒng)誤差狀態(tài)量作為系統(tǒng)的狀態(tài)量，建立六狀態(tài)阻 尼卡爾曼濾波器的狀態(tài)方程和量測(cè)方程，將慣性高度與氣壓高度之差作為組合卡爾曼濾波 器的觀測(cè)量，實(shí)現(xiàn)對(duì)慣性高度通道測(cè)量誤差的實(shí)時(shí)估計(jì)和修正，獲得大氣高度的誤差補(bǔ)償 量。該方法可以很好地跟蹤并補(bǔ)償大氣高度信息中的誤差，提高大氣高度數(shù)據(jù)可靠性，提高 慣性/大氣組合導(dǎo)航系統(tǒng)精度。
【附圖說明】
[0043] 圖1是本發(fā)明基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法的架構(gòu)圖。
[0044]圖2是垂直通道誤差傳統(tǒng)三階阻尼模型原理圖。
[0045] 圖3是一階馬爾科夫噪聲濾波補(bǔ)償效果曲線圖。
[0046] 圖4是隨機(jī)白噪聲濾波補(bǔ)償效果曲線圖。
[0047]圖5是階躍響應(yīng)濾波補(bǔ)償效果曲線圖。
[0048] 圖6是隨機(jī)白噪聲和一階馬爾科夫噪聲濾波混合補(bǔ)償效果曲線圖。
[0049] 圖7是六狀態(tài)阻尼卡爾曼濾波和傳統(tǒng)三階阻尼高度誤差對(duì)比曲線圖。
[0050] 圖8是六狀態(tài)阻尼卡爾曼濾波和傳統(tǒng)三階阻尼天向速度誤差對(duì)比曲線圖。
【具體實(shí)施方式】
[0051] 下面詳細(xì)描述本發(fā)明的實(shí)施方式，所述實(shí)施方式的示例在附圖中示出。下面通過 參考附圖描述的實(shí)施方式是示例性的，僅用于解釋本發(fā)明，而不能解釋為對(duì)本發(fā)明的限制。 [0052]如圖1所示，本發(fā)明無人直升機(jī)基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方 法的原理是:六狀態(tài)阻尼卡爾曼濾波研究方案中，包含3個(gè)慣性導(dǎo)航系統(tǒng)誤差狀態(tài)量和3個(gè) 大氣數(shù)據(jù)系統(tǒng)誤差狀態(tài)量，分別為慣性導(dǎo)航系統(tǒng)垂直加速度誤差、垂直速度誤差、垂直位置 誤差、氣壓高度表的標(biāo)度因數(shù)誤差、氣壓高度表偏置誤差和壓力偏置誤差。其中氣壓高度表 的標(biāo)度因數(shù)誤差的觀測(cè)量是由慣性阻尼后速度估計(jì)產(chǎn)生的;氣壓高度表壓力偏置誤差狀態(tài) 是直接通過壓力傳感器的偏置誤差和壓力測(cè)量值本身中的噪聲建模獲取的。對(duì)6個(gè)誤差狀 態(tài)量的實(shí)時(shí)估計(jì)，將慣性高度與氣壓高度之差作為組合卡爾曼濾波器的觀測(cè)量，通過對(duì)大 氣數(shù)據(jù)系統(tǒng)中高度測(cè)量誤差的實(shí)時(shí)估計(jì)，進(jìn)而對(duì)大氣測(cè)量誤差進(jìn)行反饋修正，最終通過大 氣輔助慣性高度通道提高在慣性/大氣導(dǎo)航系統(tǒng)中的高度測(cè)量的準(zhǔn)確性，克服傳統(tǒng)阻尼系 統(tǒng)無法滿足無人直升機(jī)在多任務(wù)飛行階段對(duì)高度信息測(cè)量的精確、可靠的高性能要求。 [0053]本發(fā)明的【具體實(shí)施方式】如下：
[0054] 1、建立無人直升機(jī)運(yùn)動(dòng)過程中大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差模型
[0055] 氣壓高度誤差模型比較復(fù)雜不易建模，通常選取氣壓高度誤差中比較典型的誤差 種類，主要為氣壓高度表的標(biāo)度因素誤差、氣壓高度表偏置誤差和壓力偏置誤差等。根據(jù)以 上幾種大氣誤差之間的關(guān)系，建立大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差模型為：
[0056] Shads - 3hb+hT3hs+3hp+UacJs+〇 ads (1)
[0057] 式（1)中Shads為氣壓高度誤差、hT為氣壓高度真值、Shb為氣壓高度表的標(biāo)度因素誤 差、Sh s為氣壓高度表偏置誤差、δ?!Ρ為壓力偏置誤差;uads為氣壓高度觀測(cè)噪聲、co ads為大氣 高度白噪聲，幾種誤差的數(shù)學(xué)表達(dá)式為：
[0061 ] S(3)*，mi = 44.32km，m2 = 0.190263，m3 = 45.395km，，m4=14.605km，
，h為無人直升機(jī)的真實(shí)高度，結(jié)合上面數(shù)學(xué)關(guān)系，為了簡(jiǎn)化無人直升機(jī)運(yùn)動(dòng) 過程中大氣數(shù)據(jù)系統(tǒng)動(dòng)態(tài)建模過程和直接說明問題，本發(fā)明設(shè)計(jì)幾種典型誤差模型作為大 氣數(shù)據(jù)系統(tǒng)氣壓高度誤差，包括隨機(jī)一階馬爾科夫誤差模型、隨機(jī)白噪聲誤差模型和階躍 響應(yīng)誤差模型，其中隨機(jī)一階馬爾科夫誤差模型：
[0063]式(4)中，HP為氣壓高度，單位米，Ητ為氣壓高度真值，Ahm為一階馬爾科夫過程誤 差，vd為飛行速度，D為馬爾科夫相關(guān)系數(shù)，coh為大氣測(cè)量誤差噪聲，一般為白噪聲。
[0064]隨機(jī)白噪聲誤差模型：
[0065] randn=AF(x) (5)
[0066] 式(5)中，A為均方差，F(xiàn)(x)為正態(tài)分布函數(shù)，randn為隨機(jī)白噪聲誤差。
[0067]階躍響應(yīng)誤差模型：
[0069]式(6)中，e(t)為階躍響應(yīng)誤差。
[0070] 2、建立六狀態(tài)阻尼卡爾曼濾波模型
[0071] (2.1)建立六狀態(tài)阻尼卡爾曼濾波狀態(tài)方程
[0072]六狀態(tài)阻尼卡爾曼濾波修正大氣高度誤差，是在傳統(tǒng)三階高度阻尼誤差回路模型 (如圖2所示)基礎(chǔ)上考慮了阻尼回路中誤差量之間的耦合關(guān)系，在高度阻尼的基礎(chǔ)上，對(duì)大 氣測(cè)量誤差進(jìn)行實(shí)時(shí)估計(jì)和補(bǔ)償。選取機(jī)體坐標(biāo)系(b系）為機(jī)體的"右、前、上"（XYZ)方向， 選取導(dǎo)航坐標(biāo)系(η系)為東北天(ENU)地理坐標(biāo)系。
[0073]如圖2所示，分別引入比例系數(shù)1^、1?、1?，反饋補(bǔ)償?shù)酱怪奔铀俣群退俣染C合計(jì)算 中，通過比例系數(shù)1^環(huán)節(jié)，使整個(gè)回路的輸出誤差保持衰減振蕩，保證系統(tǒng)的阻尼性;通過 比例系數(shù)k3環(huán)節(jié)，加快系統(tǒng)輸出誤差的衰減速度，增加系統(tǒng)的阻尼性，提高系統(tǒng)的振蕩頻 率。傳統(tǒng)阻尼回路垂直通道中各個(gè)變量的誤差關(guān)系為：
[0080] 式(7)中，11-、311_、心、6^、6113、6111)分別為氣壓高度、氣壓高度誤差、氣壓高度真 值、氣壓高度表的標(biāo)度因數(shù)誤差、氣壓高度表偏置誤差和壓力偏置誤差，Sh、SUA zja、< 分別為慣性導(dǎo)航系統(tǒng)的垂直位置誤差、垂直速度誤差、垂直加速度誤差、加速度補(bǔ)償量和舒 勒頻率， Uads、coads、ωΒ分別為氣壓高度觀測(cè)噪聲、大氣數(shù)據(jù)系統(tǒng)高斯白噪聲和慣性導(dǎo)航系 統(tǒng)高斯白噪聲誤差，1Λ為系統(tǒng)極點(diǎn)，f(h)如式(3)。
[0081] 在式（7)的基礎(chǔ)上，建立無人直升機(jī)六狀態(tài)阻尼卡爾曼濾波器的狀態(tài)方程和量測(cè) 方程：
[0083]式(8)中，X為系統(tǒng)狀態(tài)變量;為狀態(tài)變量X的一階導(dǎo)數(shù);F為系統(tǒng)矩陣;G為系統(tǒng)噪 聲系數(shù)矩陣;W為系統(tǒng)噪聲矩陣;Z為觀測(cè)量矩陣;Η為量測(cè)矩陣;V為量測(cè)噪聲矩陣。
[0084]阻尼卡爾曼濾波狀態(tài)量X為：
[0085] X=[5h 5Vz 5a 5hb 5hs 5hP]T (9)
[0086] 式(9)中，Sh、SVz、Sa分別為慣性導(dǎo)航系統(tǒng)高度通道中的垂直位置誤差、垂直速度 誤差、垂直加速度誤差狀態(tài)量;Shb、Sh s、ShP分別為大氣數(shù)據(jù)系統(tǒng)中的壓力偏置誤差、氣壓高 度表的標(biāo)度因素誤差、氣壓高度表偏置誤差狀態(tài)量。
[0087] 根據(jù)步驟1中所建立的大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差模型和式(7)，與式(9)對(duì)應(yīng)的 系統(tǒng)矩陣F為：
[0089] 式（10)中，lu、k2、k3為大氣高度三階阻尼回路比例參數(shù)。
[0090]系統(tǒng)噪聲矩陣為：
[0091] ff=[uads δΑζ ωΒ 0ads]T (11)
[0092] 式（11)中，ωΒ為測(cè)量誤差噪聲，一般為白噪聲。系統(tǒng)白噪聲隨機(jī)誤差矢量
，Vd為前向飛行速度，D為相關(guān)系數(shù)，大氣壓力噪聲co ads為高度的 函數(shù)：
[0097] (2.2)建立六狀態(tài)阻尼卡爾曼濾波量測(cè)方程
[0098] 將慣性高度h與氣壓高度心之差作為組合卡爾曼濾波器的觀測(cè)量，量測(cè)方程推導(dǎo) 如下:慣性導(dǎo)航系統(tǒng)的高度通道信息可表示為luiht+Sh，大氣高度計(jì)給出的高度通道信息 可表示為hA=ht-Sh P，量測(cè)方程即為：
[0099] Z(t) = [hi~hA] = [5h+5hp] =H(t)X(t)+V(t) = (1 0 -1 -h -l)X(t)+V(t) (14)
[0100] 式(14)中，H(t) = [l]lxl，v(t)為系統(tǒng)測(cè)量白噪聲矩陣，該模型中定為10米。
[0101] 3、無人直升機(jī)基于大氣輔助的慣性高度通道阻尼卡爾曼濾波
[0102] (3.1)將濾波器狀態(tài)方程和量測(cè)方程離散化處理：
[0104] 式（15)中，Xk為tk時(shí)刻系統(tǒng)狀態(tài)變量;Xk-i為tk-i時(shí)刻系統(tǒng)狀態(tài)變量；?k,k-i為tk-i時(shí) 刻至tk時(shí)刻系統(tǒng)的一步狀態(tài)轉(zhuǎn)移矩陣；Γ 為tk-i時(shí)刻至tk時(shí)刻系統(tǒng)噪聲系數(shù)矩陣;Wk-iS tk-i時(shí)刻系統(tǒng)噪聲矩陣;Zk為tk時(shí)刻的位置、速度及姿態(tài)觀測(cè)量矩陣;Hk為tk時(shí)刻量測(cè)矩陣;Vk 為tk時(shí)刻量測(cè)噪聲矩陣。
[0105] (3.2)在式（15)的基礎(chǔ)上加入控制項(xiàng)Uh，采用閉環(huán)修正系統(tǒng)狀態(tài)方程對(duì)慣導(dǎo)系統(tǒng) 誤差進(jìn)行修正：
[0107]式(16)中，Bk,k-1為tk-Ι時(shí)刻至tk時(shí)刻系統(tǒng)的控制項(xiàng)系數(shù)矩陣，
[0110] 上述公式中，F(xiàn)k為tk時(shí)刻系統(tǒng)矩陣的值，Gk為tk時(shí)刻系統(tǒng)噪聲系數(shù)矩陣的值，T為離 散時(shí)間，η為階數(shù)。
[0111] (3.3)得到系統(tǒng)的線性化卡爾曼濾波器方程：
[0117] 上述公式中，，為tk-i時(shí)刻至tk時(shí)刻一步預(yù)測(cè)狀態(tài)量;時(shí)刻濾波狀態(tài) 估計(jì)量；文,為tk時(shí)刻濾波狀態(tài)估計(jì)量;Kk為tk時(shí)刻濾波增益矩陣；zk為t k時(shí)刻的新息序列； Pk,k-i為tk-i時(shí)刻至tk時(shí)刻一步預(yù)測(cè)協(xié)方差矩陣;Pk-i為tk-i時(shí)刻濾波狀態(tài)估計(jì)協(xié)方差矩陣； ?k,k-l T為?k,k-l的轉(zhuǎn)置矩陣；rk-：L為tk-Ι時(shí)刻系統(tǒng)噪聲系數(shù)矩陣;Qk-Ι為tk-Ι時(shí)刻的系統(tǒng)觀測(cè) 噪聲估計(jì)協(xié)方差陣；Γ ^廠為Γ ^的轉(zhuǎn)置矩陣；Ff為Hk的轉(zhuǎn)置矩陣;Rk為tk時(shí)刻的量測(cè)噪聲 估計(jì)協(xié)方差陣;Pk為tk時(shí)刻濾波狀態(tài)估計(jì)協(xié)方差矩陣。
[0118] (3.4)用大氣數(shù)據(jù)系統(tǒng)的氣壓高度值減去步驟(3.3)估計(jì)出來的系統(tǒng)高度誤差值， 獲得大氣數(shù)據(jù)系統(tǒng)的氣壓高度修正值。
[0119] 為了驗(yàn)證發(fā)明所提出的無人直升機(jī)基于大氣輔助的慣性高度通道阻尼卡爾曼濾 波方法的正確性及有效性，采用本發(fā)明方法建立六狀態(tài)阻尼卡爾曼濾波模型，用Matlab進(jìn) 行阻尼卡爾曼濾波，對(duì)產(chǎn)生的濾波修正量和加入的氣壓高度誤差量進(jìn)行仿真驗(yàn)證對(duì)比。對(duì) 一階馬爾科夫誤差、隨機(jī)白噪聲誤差、階躍響應(yīng)誤差、隨機(jī)白噪聲和一階馬爾科夫噪聲混合 誤差的跟蹤和補(bǔ)償?shù)姆抡娼Y(jié)果如圖3-圖6所示。
[0120] 在無人直升機(jī)動(dòng)態(tài)飛行過程中，加入一階馬爾科夫噪聲的氣壓高度誤差，進(jìn)行慣 性/大氣組合導(dǎo)航系統(tǒng)仿真，采用傳統(tǒng)三階阻尼和六狀態(tài)阻尼卡爾曼濾波得到的高度誤差 和天向速度誤差對(duì)比如圖7-圖8所示。
[0121] 圖3、圖4和圖6中灰色實(shí)線表示仿真中加入的氣壓高度誤差量，黑色實(shí)線表示采用 本發(fā)明的經(jīng)過卡爾曼濾波得到的氣壓高度誤差量；圖5中黑色實(shí)線表示仿真中加入的氣壓 高度誤差量，黑色虛線表示采用本發(fā)明的經(jīng)過卡爾曼濾波得到的氣壓高度誤差修正量。從 圖中可以看出，采用本發(fā)明阻尼卡爾曼濾波得到的氣壓高度誤差修正量和大氣數(shù)據(jù)系統(tǒng)中 本身加入的一階馬爾科夫誤差、隨機(jī)白噪聲誤差、階躍響應(yīng)誤差、隨機(jī)白噪聲和一階馬爾科 夫噪聲混合誤差趨勢(shì)一致，所以此卡爾曼濾波器模型合理，可以一定程度修正大氣數(shù)據(jù)，為 后續(xù)輔助慣性導(dǎo)航提供了依據(jù)。圖7-圖8中黑色實(shí)線表示加入一階馬爾科夫誤差以后采用 本發(fā)明六狀態(tài)阻尼濾波得到的高度誤差和天向速度誤差，灰色實(shí)線表示采用傳統(tǒng)三階阻尼 模型得到的高度誤差和天向速度誤差，從圖中的對(duì)比曲線可以看出，本發(fā)明的六狀態(tài)阻尼 濾波方法的高度誤差精度相比傳統(tǒng)三階阻尼方法提高約2倍，天向速度誤差精度也有提高， 因此設(shè)計(jì)結(jié)合經(jīng)典阻尼特性的六狀態(tài)卡爾曼濾波器，可以很好的跟蹤并補(bǔ)償大氣高度信息 中的誤差，提高大氣高度數(shù)據(jù)可靠性，提高慣性/大氣組合導(dǎo)航系統(tǒng)精度。
[0122]以上實(shí)施例僅為說明本發(fā)明的技術(shù)思想，不能以此限定本發(fā)明的保護(hù)范圍，凡是 按照本發(fā)明提出的技術(shù)思想，在技術(shù)方案基礎(chǔ)上所做的任何改動(dòng)，均落入本發(fā)明保護(hù)范圍 之內(nèi)。
【主權(quán)項(xiàng)】
1. 基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，其特征在于，包括如下步驟： 步驟1，建立無人直升機(jī)運(yùn)動(dòng)過程中大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差模型，所述大氣數(shù)據(jù)系 統(tǒng)的高度誤差為氣壓高度表的標(biāo)度因素誤差、氣壓高度表偏置誤差和壓力偏置誤差； 步驟2,在步驟1對(duì)無人直升機(jī)運(yùn)動(dòng)過程中大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差建模的基礎(chǔ)上， 將慣性導(dǎo)航系統(tǒng)高度通道的三個(gè)誤差量和大氣數(shù)據(jù)系統(tǒng)的三個(gè)高度誤差量作為系統(tǒng)的狀 態(tài)量，構(gòu)建無人直升機(jī)六狀態(tài)阻尼卡爾曼濾波器的狀態(tài)方程和量測(cè)方程； 步驟3,對(duì)步驟2建立的狀態(tài)方程和量測(cè)方程進(jìn)行離散化處理以及狀態(tài)量、量測(cè)量的更 新，實(shí)現(xiàn)對(duì)大氣高度測(cè)量誤差的跟蹤和補(bǔ)償。2. 根據(jù)權(quán)利要求1所述基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，其特征在 于，步驟1所述大氣數(shù)據(jù)系統(tǒng)氣壓高度誤差模型為： Shads - 3hb+hT3hs + 3hp+Uacls+ CO ads， 其中，Shads為氣壓高度誤差、Shb為氣壓高度表的標(biāo)度因素誤差、hT為氣壓高度真值、Sh s 為氣壓高度表偏置誤差、Shp為壓力偏置誤差、uads為氣壓高度觀測(cè)噪聲、《ads為大氣高度白 噪聲;3hb、3hs、5h p的數(shù)學(xué)表達(dá)式;^1·其中，， .，h為無人直升機(jī)的真實(shí)高度，mi = 44.32km，m2 = 0.190263,m3 = 45.395km,,1114=14.605km,丨系統(tǒng)極點(diǎn)，ωB為測(cè)量誤差噪 聲。3. 根據(jù)權(quán)利要求1所述基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，其特征在 于，步驟2所述無人直升機(jī)六狀態(tài)阻尼卡爾曼濾波器的狀態(tài)方程為：其中，X為系統(tǒng)狀態(tài)變量、X為狀態(tài)變量X的一階導(dǎo)數(shù)、F為系統(tǒng)矩陣、G為系統(tǒng)噪聲系數(shù)矩 陣、W為系統(tǒng)噪聲矩陣，狀態(tài)方程表示為：\ f j \ j w - v v / 其中，δΚδνζ、δ&分別為慣性導(dǎo)航系統(tǒng)高度通道中的垂直位置誤差、垂直速度誤差、垂直 加速度誤差狀態(tài)量;3hb、3hs、5hP分別為大氣數(shù)據(jù)系統(tǒng)中的氣壓高度表的標(biāo)度因素誤差、氣 壓高度表偏置誤差、壓力偏置誤差狀態(tài)量;分別為大氣高度三階阻尼回路比例參 數(shù)，，:h為無人直升機(jī)的真實(shí)高度，mi = 44.32km，m2 = 0.190263,m3 = 45.395km,m4= 14.605km為系統(tǒng)極點(diǎn)，為舒勒頻率，uads 為氣壓高度觀測(cè)噪聲JAZ為垂直加速度誤差，ωΒ為測(cè)量誤差噪聲，coads為大氣高度白噪聲。4. 根據(jù)權(quán)利要求1所述基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，其特征在 于，步驟2所述無人直升機(jī)六狀態(tài)阻尼卡爾曼濾波器的量測(cè)方程為： Z = HX+V, 其中，Z為觀測(cè)量矩陣、Η為量測(cè)矩陣、X為系統(tǒng)狀態(tài)變量、V為量測(cè)噪聲矩陣;量測(cè)方程表 示為： Z(t) = [hi_hA]=H(t)X(t)+V(t) = (l 0 -1 _h _l)X(t)+V(t)， 其中，hi = ht+5h，hA=ht_5hP，H(t) = [l]ixi，V(t)為系統(tǒng)測(cè)量白噪聲矩陣，h為無人直升 機(jī)的真實(shí)高度，Sh為慣性導(dǎo)航系統(tǒng)高度通道中的垂直位置誤差，ShP為壓力偏置誤差，t為時(shí) 間。5. 根據(jù)權(quán)利要求1所述基于大氣輔助的慣性高度通道阻尼卡爾曼濾波方法，其特征在 于，所述步驟3的具體過程為： 31) 將濾波器狀態(tài)方程和量測(cè)方程離散化處理得到：其中，Xk為tk時(shí)刻系統(tǒng)狀態(tài)變量，Xk-Atk-i時(shí)刻系統(tǒng)狀態(tài)變量，Ok.k-Atk-i時(shí)刻至tk時(shí) 刻系統(tǒng)的一步狀態(tài)轉(zhuǎn)移矩陣，Γ 為tk-i時(shí)刻至tk時(shí)刻系統(tǒng)噪聲系數(shù)矩陣，Wk-i為tk-i時(shí)刻 系統(tǒng)噪聲矩陣，Zk為tk時(shí)刻的位置、速度及姿態(tài)觀測(cè)量矩陣，Hk為tk時(shí)刻量測(cè)矩陣，Vk為tk時(shí) 刻量測(cè)噪聲矩陣； 32) 對(duì)離散化處理得到的公式加入控制項(xiàng)Uk-i，并采用閉環(huán)修正系統(tǒng)狀態(tài)方程對(duì)慣導(dǎo)系 統(tǒng)誤差進(jìn)行修正：其中，Bk, k-A tk-i時(shí)刻至tk時(shí)刻系統(tǒng)的控制項(xiàng)系數(shù)矩陣； 33) 得到系統(tǒng)的線性化卡爾曼濾波器方程：其中，之為tk-1時(shí)刻至tk時(shí)刻一步預(yù)測(cè)狀態(tài)量;之_^tk-l時(shí)刻濾波狀態(tài)估計(jì)量;元為 tk時(shí)刻濾波狀態(tài)估計(jì)量;Kk為tk時(shí)刻濾波增益矩陣;Zk為tk時(shí)刻的新息序列;Pk^Atk-i時(shí)刻 至tk時(shí)刻一步預(yù)測(cè)協(xié)方差矩陣;Pk-Ι為tk-1時(shí)刻濾波狀態(tài)估計(jì)協(xié)方差矩陣；?k,k-i%?k,k-i的 轉(zhuǎn)置矩陣；時(shí)刻系統(tǒng)噪聲系數(shù)矩陣;時(shí)刻的系統(tǒng)觀測(cè)噪聲估計(jì)協(xié)方差 陣；Γ ^廠為Γ ^的轉(zhuǎn)置矩陣;把為Hk的轉(zhuǎn)置矩陣;Rk為tk時(shí)刻的量測(cè)噪聲估計(jì)協(xié)方差陣;Pk 為tk時(shí)刻濾波狀態(tài)估計(jì)協(xié)方差矩陣;I為單位矩陣； 34)根據(jù)33)得到的線性化卡爾曼濾波器方程估計(jì)系統(tǒng)高度誤差值，并用大氣數(shù)據(jù)系統(tǒng) 的氣壓高度值減去系統(tǒng)高度誤差值，得到大氣數(shù)據(jù)系統(tǒng)的氣壓高度修正值。
【文檔編號(hào)】G01C21/16GK105865446SQ201610352438
【公開日】2016年8月17日
【申請(qǐng)日】2016年5月25日
【發(fā)明人】戴怡潔, 熊智, 王融, 邢麗, 吳璇, 許建新, 施麗娟, 孔雪博, 唐攀飛, 趙宣懿, 萬眾, 鮑雪, 閔艷玲, 黃欣, 張苗, 羅瑋, 李博, 李一博
【申請(qǐng)人】南京航空航天大學(xué)