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