TW531657B - Enhanced integrated positioning method and system thereof for vehicle - Google Patents

Description

531657 五、發明說明（1) 王球疋位系統（G p s )是一基於衛星，承天 候被動式益繞命—，< x 復盍全球的玉 勑式…、、杲包疋位和授時系統； 防部開發、運行和管理，哕季續一/卜位糸統由美㈣ 中、巧地、洛、… 該糸、、先故计來為全球範圍的空 ,,陸 海年上的裝有GPS接收機的飛機、f # 55、戰 艦、商用船隻、運動鱼鉍以芬广A # %機航工押 ^ 和時間信息。 捉仏精岔的位置、逯度 王球定位系統接收設備包括一個接收天線、一個信號

二及相關的電子線路和必要的顯示設備；全球 =位糸，，接收機接收GPS衛星信號，纟導出飛行禮如飛 彈、火箭的位置、速度和時間信息；全球定位系統 定，是基於測距的原理，衛星的位置可以由衛星的星曆表 解算出來，而衛星到用戶的距離可以由全球定位系統接收 機測得的衛星信號時間延遲導出來；衛星信號以光速傳 播，衛星到GPS接收機的幾何距離等於衛星信號時間延遲 乘以光速；這樣測得的幾何距離叫做偽距，它包含了很多 的誤差，例如接收機的時鐘誤差；因為衛星用的是原子 鐘’它的誤差遠遠小於GPS接收機的時鐘誤差；在三維飛 行體導航系統中，這個未知的用戶接收機的時鐘誤差也要

解出來’因此應用早一的全球定位糸統導航需要至少四顆 GPS衛星觀測量。 全球定位系統的誤差源有：信號傳播誤差、衛星時鐘 |誤差和星曆表誤差、以及選擇可用性（Selective Αν ail ability)誤差；用戶距離誤差（U RE)就是由這些誤 差源引起的沿用戶到衛星視線上的測距誤差；全球定位系

第6頁 531657

五、發明說明（2) 統誤差長時s保持穩定，這就是全球定位系統能提供 ，疋的f航解的原因；然而全球定位系統的缺點是；很 谷易被有思或無意地干擾或被欺編，纟 ^ ，接收機的天線也有可能被遮擔而導致接收二T 仏唬衛星信號信噪比低和飛行體作大機動

能導致衛星信號的失鎖。 U J

一慣性導航系統由一慣性測量單元、一個處理哭、和 一套嵌入式的導航軟件組成；定位解通過數值解由i行體 的比力和轉動速率建立的Newton運動方程’比力和轉動速 率由t貝性傳感器測定，慣性傳感器包括加速度計和陀螺 儀，它們連同相關的硬體和電子線路一起組成慣性測量單 元0 个貝性導航系統包括框架式和捷聯式兩種；在框架式慣 性導航系統中加速度計和陀螺儀被固定在一框架平台上以 隔離傳感器，從而保證觀測量和導航計算都處於一穩定的 導航座標系中；可能的導航座標系包括地心慣性座標系 (Earth Centered IntertaU-ECI)，地心地固座標系 (Earth Centered Earth Fixed-ECEF)，北東下局部座標

系（North East Down-NED)，游移方位局部座標系；在捷 聯慣性導航系統中，慣性傳感器被鋼性地連接在飛行體 上，而一座標系轉換矩陣則用於將表達為機體的加速度和 轉動觀測罝轉換為導航座標系中相應的量，並在一穩定的 導航座標系中作導航計算，這個座標系轉換矩陣又被稱為 分析平台，框架式慣性導航系統可以比捷聯式導航系統更

式導航系 動可能影 計的出現 成本低， 航系統允 初始對準 和姿態信 還包括全 ，其導航 系差隨時 誤差源引 子誤差。 和單獨的 在一些限 輸出，抗 統不能滿 慣性系統 輔助，利 糸統的導 航彌補慣 導航糸統 收機的位 全球定位 統受制於高 響慣性傳感 ，捷聯式導 同時性能可 許純粹自動 後慣性導航 息，除了自 導航解和寬 解並隨時間 間不斷地增 起，例如陀

χ …--------------------------------S 五、發明說明（3) 精密，更易於校正；接胳 隨宴=速率的轉動，高機 1新的陀螺儀和加 &&地位，因為它的 处原理上講，慣 :起點位置初始化和姿： ;的，體的位置、：； 卜性導航系統的優二 ”性導航系統價格昂貴 義疋慣性導航系統 差特性主要由慣性傳；； 加速^計偏差，和刻度因 單獨的慣性導航系統 自的缺點，這些缺點表明 本，南精度，連續導航解 動等’獨立的一個導航系 對於全球定位系統與 說，這兩個導航系統互相 導航高精度彌補全球定位 系統的長時間的高精度導 間增長的缺點。 全球定位系統和慣性 就是應用全球定位系統接 系統，第二種組合方式為 機動環境， 器的性能， 航系統越來 罪。 操作，在進 糸統輸出連 動化操作 的帶寬；然 漂移，其含 加’這種誤 螺儀漂移， 全球定位系統都存在各 制條件下，例如低成 干擾能力，和適應大機 足任務要求。 組成的組合導航系統來 用慣性導航系統的短時 航誤差，利用全球定位 性導航系統的誤差隨時 的一種簡單的組合方式 置和速度直接重調慣性 系統和慣性系統的級聯 531657 五、發明說明（4) 一 、、且合」在這種組合方式中全球定位系統導出的位置和速度 做$觀测量’ 一組合Ka丨man濾波器融合來自慣性導航系統 ^ ^球定位接收機的導航解；第三種全球定位系統和慣性 破，系先的組合方式為應用一擴展的Ka 1 man濾'波器處理全 =义位系統接收機的偽距和偽距率等原始觀測量以及慣性 ^航系統的信息，從而得到最優的慣性導航系統誤差、慣 · 性傳感器誤差和GPS接收機鐘偏移的估計。 目月ίι的全球定位系統和慣性導航系統的組合系統存在 的缺點有： 1 ·在傳統的全球定位系統和慣性導航系統的組合系統 ，只有由GPS接收機導出的位置和速度信息、或Gps原始 塢距和偽距率被用來同IMU觀測量做組合；實際上，Gps接 收機‘出的GPS載波相位具有很高的距離測量精度，由於 f難求解載波相位模糊值，故而GPS载波相位還未被於 水解GPS/IMU組合導航解。 ； 2. 用慣性導航系統輔助GPS接收機跟縱環路的— 憂是這可能引起傳統的GPS/INS組合導航系統的不:，隱 其原因是在GPS/INS組合系統中存在一個正反饋迴^疋’ 確度不高的慣性輔助信息會增大GPS信號跟縱誤差·’精 繞合Ka 1 man濾波器是根據已知慣性系統的性处°' ’因為 GPS觀測量會嚴重地影響組合Kalman濾波的地、周的’ ~大後的GPS跟蹤誤差又反饋回慣性系統弓丨 口而 性能進一步變壞。 貝生系統的 3. 傳統的緊密型GPS/INS組合導航系統 、、、、甲，低精度的

531657 五、發明說明（5) ~~ ~一~ -一·'—-- ^傳感器不能用於GPS接收機的載波相位跟縱辅助，因 為載波跟蹤迴路要求高精度的外來輸入的速度輔助。 ”Λ!統的鬆散型全球定位系統和慣性導航系統的組合 四顆GPS衛星信號，這是因為gps接收機 ，求跟蹤至少四顆GPS衛星才能導出載體的位置和速产 =^而GPS位置和速度是進行鬆散型全球定位系統與$ ‘ V航系統組合所必需的觀測量，這個要求限制了 GPS/ IMU組合系統的應用範圍，鬆散型全球定位系^統 性導航系統的組合系統的另一個缺點是它的動熊特性貝 好；由於沒有外部傳感器輔助GPS接收機的碼和〜波 跟蹤環路當載體作大機動運動時GPS接收機會位 信號的跟蹤。 ^ ^衛星 5.由於GPS/IMU組合系統導航精度主要由Gps定位 來保持，然而同時GPS接收機在高度上的定位精度月度 從而導致G P S / I Μ ϋ組合系統在高度上的導航精产不古 本發明之主要目的是提供一增強型導航定位方去 系統，該方法和系統將來自一高度測量裝置（如一恭及其 度計）的高度觀測量融入GPS/IMU組合導航定達向 高GPS/IMU組合導航系統在高度上的導航精確度者者 本發明的又一目的是提供一增強型導航定&位方。/ 統，該方法和糸統將該南度測量裝置，如—雷達古 和系 測得的高度信息融入鬆散型GPS/ IMU組合系統中來$ ’ 合導航系統的高度上的導航精度者。 ΑΑΘ向k 531657 五、發明說明（6) ——~ ^ 輔助的導航定位方法和系統，其中來自慣性導航處理器的 速度和加速度信息用於輔助GPS接收機的碼和載波相位"跟 縱環路，以增強組合導航系統的導航性能，特別是提高系 統抗干擾的能力和承受大動態的能力，防止在“^接收^機、 中常常踫到到的衛星信號失鎖和載波相位跳周者。 本發明的另一目的是提供一增強型該高度測量裝置輔 助導航定位方法及其系統，其中慣性導航系統通過提供更 · 精禮的位置k息來輔助G P S衛星信號的載波相位模糊度求 -解者。 | 本發明的另一目的是提供一增強型該高度測量裝置輔鲁 |助導航定位方法及其系統，其中GPS接收機、慣性測量單 | 元（IMU)和該咼度測量裝置的組合導航解用於提供更精確 !的位置彳§息和誤差協方差矩陣來輔助GPS衛星信號的載波 |相位模糊度求解者。 ： 本發明的另一目的是提供一增強型該高度測量裝置輔 |助導航定位方法及其系統，GPS衛星信號的載波相位觀測 丨值同偽距和偽距率一起用於卡爾曼濾波來提高組合導航解I I的導航精度者。 f !

! I I 本發明的另一目的是提供一增強型該高度測量裝置輔丨 |助導航疋位方法及其系統，實時卡爾曼濾波器被用來最優鲁 I地融合GPS原始數據、慣性導航解和高度觀測量者。 本發明的另一目的是提供一增強型該高度測量裝置輔 -助的導航定位方法及其系統，其中一穩健的卡爾曼濾波器 · 用於消除組合系統中潛在的不穩定因素者。

第11頁 531657 五、發明說明（7) 本發明的另一目的是提供一增強型該高度測量裝置輔 助的導航定位方法及其系統，由於GPS接收機和高度測量 值輔助，低精度的慣性測量傳感器可以用於組合系統來獲 取南精度的組合導航解者。 圖示說明： 第1圖：係說明一優選實現方案的增強型該高度測量 裝置輔助導航定位方法及系統、慣性觀測量、高度測 量值、和G P S觀測量在一中心導航處理器中作融合之 方塊圖。 第2a圖：係說明一根據第一優選實現方案的全球定位 糸統的處理過程，該中心導航處理器用於外部輔助之 方塊圖。 第2b圖：係說明一根據第二優選實現方案的全球定位 t t的處理過程，該中心導航處理器用於外部輔助之 方塊圖。 ，丄係說明一根據第三優選實現方案的全球定位 不、、.充的處理過程之方塊圖。 二3: J姑係]兒明一’艮據上述第一優選實現方案的全球 辅助ίί的信號處理過程，中心導航處理器用於外部 祠切I万塊圖。 係？明一根據上述第二優選實現方案的全球 '、、、的信號處理過程，該中心導航處理器用於外

第12頁 531657 五、發明說明（8) 部輔助之方塊圖。 第3 c圖：係說明一根據上述第三優選實現方案的全球 定位系統處信號處理過程之方塊圖。 第4a-l圖：係說明一根據上述第一優選實現方案的中 心組合導航處理過程，以及它與該全球定位系統 (GPS )處理器，該慣性測量單元和該高度測量裝置之 . 間的關係之方塊圖。 _ 第4a-2圖：係說明一根據上述第一優選實現方案的中 心組合導航處理過程，以及它與該全球定位系統 i (GPS )處理器，該慣性測量單元和雷達高度計之間的 _ 關係，並說明一數據融合模塊之方塊圖。 第4b-l圖：係說明一根據上述第二優選實現方案的中 心組合導航處理過程，以及它與該全球定位系統 (GPS)處理器、該慣性測量單元和該高度測量裝置之 i

I

間的關係之方塊圖。 I 第4b-2圖：係說明一根據上述第二優選實現方案的中 I 心組合導航處理過程，以及它與該全球定位系統 | (GPS)處理器、該慣性測量單元和雷達高度計之間的 I 關係，並說明一數據融合模塊之方塊圖。 # 第4c-1圖：係說明一根據上述第三優選實現方案的中 心組合導航處理過程，以及它與該全球定位系統 (GPS)處理器、該慣性測量單元和該高度測量裝置之 間的關係之方塊圖。 ^ 第4c-2圖：係說明一根據上述第三優選實現方案的中

第13頁 531657 五、發明說明（9) 心組合導航處理過程，以及它與該全球定位系統 (GPS)處理器，該慣性測量單元和雷達高度計之間的 關係，並說明一數據融合換塊之方塊圖。 第5圖：係說明一根據上述三種優選實現方案的慣性 導航處理過程，導航狀態校正量來自卡爾曼濾波器之 方塊圖。 第6圖：係說明一根據上述三種優選實現方案的穩健 | 的卡爾曼濾波器之方塊圖。 I 第7圖：係說明一根據上述第一優選實現方案的GPS衛 星信號載波相位模糊度求解，組合導航解用於輔助 4 GPS衛星信號載波相位模糊度求解之方塊圖。 i !

I ! ! 丨圖號說明： 丨 j ! j j

10 —慣性測量單元（ΙΜϋ) 20 —高度測量裝置 I 丨 21 —雷達高度計 26 —振蕩器電路 I 30 —全球定位系統（GPS)處理器 丨 ； ί j 31 —全球定位系統天線 丨 32 —前置放大電路 33 —下變頻器 y 3 4 — I F採樣和A / D轉換器 3 5 —信號處理器 3 5 1 —都卜勒移去器 3 5 2 —相關器 3 5 3 —累加器 3 54 —微處理器 355 —載波NCO 356 —碼產生器 3 5 7 —碼的數控振蕩器電路（NCO)

第14頁 53l657 五、發明說明 36 —振蕩器電路 37 —全球定位系統GPS導航處理器 4 0 —中心導航處理器 口 41 一慣性導航系統（INS)處理器 411 —IMU輸入/輸出（1/〇)接口 412 —IMU誤差補償模塊 m , 4U —姿態、位置速度計算模^坐標轉換計算模塊 415 一轉換矩陣4鼻模塊 4 1 6 —地球和運載體轉動速率計算模 4 2 —載波相位整周模糊求解模塊 421 —幾何距離計算模塊422 —最小平方，敕士 424 —電離層模型 426 —衛星預測模塊 43 ^爾曼濾波器 432 —協方差傳播模塊 434 —協方差更新模塊 =-狀態向量預測模塊 4 3 8 —觀測量焱 μ —齡μ 1殘差計算模塊 4 4 一數據融合模塊 5〇一輸入/輪出“/0)接口 42 3 —衛星鐘模型 t 千方4 ι模塊 4 2 5 —對流層模型 4 2 7 —收索空間確定模塊 4 3 1 —殘差監視模塊 4 3 3 —最優增益計算模塊 4 3 5 —預處理模塊 4 3 7 — G P S誤差補償模塊 4 3 9 —狀態向量更新模塊 4 5 —地形數據庫 說明’· 本發明係-增強型該高度測量裝置輔助的導航定位方

第15頁 五、發明說明（11) 法及其系統；該方法 慣性導航系統中、馬’ 糸統可從根本上解決在GPS導航和 GPS易於被干擾和、^的各種問題’如GPS衛星信號丟失、 度導航數據精户差人·編#、慣性導航解隨時間漂移、以及高 自GPS接收機、^ 發明應用一卡爾曼濾波器來融合來 量，該發明更進一器和該高度測量裝置的原始觀測

觀測量、慣性導#二f :穩健的卡爾曼濾波器來融合GPS 的高度觀測量該Ϊ度測量裝置如雷_^^ i統GPS/IMU誕人爾哭濾波器可從根本上消除傳 I σ糸統中存在的不穩定性。 丨 本舍明可用於航太領 丨它也可用於飛機、可復】航空器的精密導航， 丨呈的精密淮+ 運載火則以及其他空中交通工 i航和跟蹤。者陸’也可被適當修改以用於地面車輛導 i GPS丄參導閱二圖所示’一增強型該高度測量裝置輔助的 ! GPS/IMU泠舡定位系統包括一慣性測量 一上☆扪 丨量裝置20、和一全球定位李 兀丨〇、一向度測 丨中、、邕如本王琢疋位糸統（GPS)處理器30、它們都盥 丨一中〜V航處理器40相連，導航解 ^ 丨接口50輪出。 、輸入/輸出（I/O) i : 請續參閱第1、2a、3a、4a-1、4a —2、5 :說明本發明的第一優選實現方案；第—實、y括 I如下步驟： 貝兄1定璉包括 ! (1)進行Gps處理，並從該全球定位系統（GPS)處理哭 丨30接收GPS觀測量’它們是偽距1波相位和都 。。 移’並將它們送給該中心導航處理器4 〇。 八

531657 I ' — — — _______ ______ ____ I五、發明說明（12) I (2)從該慣性測量單元i〇接收慣性觀測量，它們是機 體轉動角速率和比力’並將它們注入該中心導航處理琴40 的慣性導航系統(INS)處理器41。入該中 (3 )從該南度測里震置2 〇接收高度觀測量，並將它送 給該中心導航處理器4 0。 (4) 用該卡爾曼/慮波器43融合ins處理器41的輸出、高 度觀測量和GPS觀測量。 ° (5) 執行慣性導航處理，並將卡爾曼濾波器43的輸出 |反饋給INS處理器41來校正ins導舨解。

； (6)將1NS處理器41輸出的速度和加速度數據注入該全 I球定位系統（GPS)處理器30的微處理器354來輔助全球定位 j系統衛星信號的碼和載波相位跟蹤。 | (7)將該全球定位系統（GPS)處理器30的微處理器354 '的輸出、INS處理器41的輸出、以及卡爾曼濾波器43的輪 出达給一載波相位整周模糊求解模塊42來求解Gps衛星信 ；號載波相位整周模糊值。 ° ；^ (8)將載波相位整周模糊值由載波相位整周模糊求解 I杈塊送給該卡爾曼濾波器43以進一步提高導航精度。 (9)將導航數據由ins處理器41輸出到輪入

j =，其他機載航空電子系統可從該輸入/輪出接口5〇獲取 |導航數據。 在第（1 )步，G P S衛星在L波段射頻1上廣播粗捕獲碼 d/A )和精確碼（p )，該讣3衛星信號為：

第17頁 531657

五、發明說明（13) A! (0 二 CA(t)D(t) cos(^y!t + φ) + -f φ) G P S衛星在L波段射頻2上廣播精確碼（p ) ，$亥 星信號為： Λ S衛 I 其中：㈧是1^波段上射頻的載波頻率；0是〜/ 丨量，它是相位噪聲和晶體振蕩器電路漂移的和；p。: |碼信號功率；？？是？碼信號功率；D(t)是調制的導教=C/A I I據；CA(t)是C/A碼；P(t)是P碼；％是[2波段射頻 1波頻率；是L2-P碼信號功率；02是一小量，它是广栽 ；聲和晶體振蕩器電路的漂移的和。 X目位噪 ! 請參閱第2a圖所示，在第（1)步驟中，GPS接收機、 |球定位系統天線31接收到的衛星信號為： 、的全 i : i I (__ ! (0 - ^2PCCAO - T)D(t)ο〇5[(ω1 + ωj}/ + φ)] + ^P{i)D(t)sin[(c〇] ： (〇 = ^[2RP(t - r)D(i)cos[(〇)2 + φΊ)] " ] | 其中：T是碼延遲；是都卜勒頻移。 ! 請續參閱第2a圖所示，在第（1)步驟接收到的GPS信號 |由一前置放大電路32放大，被放大了的GPS信號送給該全

第18頁 531657 五、發明說明（u) 球定位系統（GPS )處理器3 〇的該下變頻器3 3，該下變頻器 3 3轉換射頻（RF )信號為中頻（丨F )信號，中頻信號再由 IF採樣和A/D轉換器34轉換為正交的；[(in —phase)和Q (quadraphase)分量數據；在11?採樣和A/D轉換器34中， 中頻信號先由一低通濾波器濾波，然後被採樣，最後才 由模擬信號轉換為數字數據，數字數據輸入一信號處理器 3 5，該信號處理器3 5從這些數據中解調出導航數據，它們 是GPS衛星星曆表、大氣層參數、衛星鐘參數和時間信 息；該信號處理器35還從這些數字數據中導出偽距、載波 相位和都卜勒頻率，在該全球定位系統（Gps)處理器3〇

中，一星曆表提供時鐘信號給該下變頻器33、IF採樣和 丨A/D轉換器34及該信號處理器35。 I i t 請參閱第2a圖所示，在第（1)步驟的該信號處理器35 ! |輸出GPS觀測量給該中心導航處理器4〇，這些Gps觀測量是 |偽距、載波相位和都卜勒頻率；在第（6)步驟該信號處理| \器35從該中心導航處理器4〇接收速度和加速度信息，並執 |行外來速度辅助的GpS碼和載波相位跟蹤算法。 I 偽距觀測量從GpS接收機的碼跟蹤環路導出，如第3a j 圖所示，碼跟蹤環路包括一相關器3 52、一累加器3 53、一 ! 微處理器354、一碼的數控振蕩器電路（numericai i controlled oscillator-NCO)357和一碼產生器356 ;都卜 勒頻移和載波相位由GPS衛星信號的載波跟蹤環路得到， |載波跟蹤環路包括一都卜勒移去器351、一相關器352 、 累加态3 5 3 —微處理器3 5 4和一載波數控振蕩器電路

第19頁 531657 五、發明說明（15) 3 55 ° 來自IF採樣和a/d轅施栗 去器351處理，從而移去=數據由該都卜勒移 都卜勒頻率移去器由—數字單邊帶；：ί:都卜3頻率’ 器滿時產生一新的週期： = 累相&，每次當積累 白兮料卢 / 振芴器電路2 6的時鐘和來 ^ 口、 的心11:&頻率一起驅動該載波NC03 55，_ 載波圆55輸出參考信號的旧分量（即 5亥都卜勒移去器3 5 1。 '

經都卜勒移去處理後的GPS衛星信號送入該相關器352 與本地產生的偽隨機碼作相關處理，該累加器3 53緊跟該 相關器35 2之後，它對該相關器3 52的輸出作相關處理並濾 波，累加處理即將T時間間隔中的相關後的數據累加起 來，T通常取為c / A碼的一個碼片長，累加後的數據（〖3和 Q 3 )送給該微處理器3 5 4，接著倒空該累加器，這即為信 號的累加-倒空濾波。

用於該相關器3 5 2中作相關運算的本地碼由該碼產生 器3 5 6產生，而該碼產生器3 5 6則由來自該振蕩器電路3 6的 時鐘和來自該微處理器354的delta延遲驅動，該碼產生器 3 5 6用於產生C/A碼和P碼，該碼NC0357由該振蕩器電路36 和遠被處理3 5 4驅動，該碼N C 0 3 5 7輸出的時鐘再驅動該 累加器353，該碼NC0357還驅動該碼產生器3 5 6。 I 請續參閱第3a圖所示，在第（6)步該微處理器從該累 I加器3 5 3接收數據和從該中心導航處理器40接收速度和加

I

第20頁 531657 五、發明說明（16) 速度“息並進行環路濾波處理、信號搜 測、數據恢復和觀測量處理，這種工作模：：：檢 度辅助的載波相位跟蹤和碼相位跟蹤；在^ (丨驟&中心逮 微處理器輸出偽距、載波相 /由中垓 處理器40。 ㈣卜勒頻率到該中心導航 在第（1)步驟中，當GPS信號跟蹤誤差 路的跟蹤帶寬時，•星信號會丢失，跟蹤環“以， k唬的跟蹤主要由低的信號噪聲比（ ^ f信號都卜勒頻移引起，前者主要由輸入噪和聲接和收^ 生而後者則主要由飛行體的高速運動引起，一般 擴展跟縱環路的帶盲可η兮盖错}日p + ::m玄善鎖相％在高機動環境中的跟 蹤〖生此，迫種方法同時引入更多的噪聲從而降低了Gps接 。=欠=，，利用校正後的慣性導航系統解輔助 GPS接收機的跟蹤環路既可提擴展跟蹤環的跟蹤 時又能提高其抗干擾的能力。 、 請參閱第3a圖所示，在第（6)步驟中被校正後的慣性 導航系統的速度和加速度信息輔助GPS鎖相環的目的是在 ^夠短的時間内對中頻信號的相位作精確的估計； 這個中頻信號Θ〆t)的載波相位可以近似為： + _

/0 +0/〇/+ W +0、/0/J 於是問題變為估計上述等式的參冑，將描述飛行體運

531657 發明說明（π) 動的速度和加速度投影到衛星視線（L〇S )方向上，於是 中頻信號的載波相位可以由視線方向上的速度和加速'度^表 示為： 、 r +.. e{t)^bxVLOSt + b2ALosf +bM^〇s 其中：，bz，b3)是與載波頻率和光速有關的常 量，它們為： 3c yL.os，和 aL〇s 距離加速度和距離 星信號跟蹤性能和 度的估計之精確度 以由慣性導航系統 它們注入該微處理 該信號處理器 展頻譜信號的碼相 計，該信號時延在 衛星的距離估計， 分別對應於沿接收機到衛星的距離率， 力^速度率，因此輔助的(；]?8接收機的衛 抗干擾的能力依賴於距離率和距離加速 ;視線方向上的距離率和距離加速度可 中的速度和加速度信息計算出來，並把 器3 5 4的環路濾波器中。 35的碼跟蹤環路跟蹤輸入的直接序列擴 位，碼跟蹤環提供衛星信號時延的估 該微處理器354中被用來計算飛行體到 它即為偽距（pseud〇range)，來自該中

531657 五、發明說明（18) 。導航處理WG的速度和加速度信息 速度和加速度信自，親綠Η^ ☆将禊马視、，泉方向上的 ^ Ψ Μ心見、、泉上的速度和加速度用來輔助確定 =馬的#延’從而增強Gps接收機的動態性能和抗干擾 >^GPS提仏的阿度導航彳§息精度較差，而αν〗腳組合 航系統的長時間精度指標主要依賴於Gps的 " 此奶簡組合不能改善系統在垂直方向上的導航性能口 I即南度上的導航精度；本發明利用一高度測量裝置來彌補 i G P S / IM U組合糸統的這一缺點。 j

I 請參閱第4a-1圖所示，該中心導航處理器40，它融合 |來自該慣性測量單元的慣性觀測量、來自該全球定位系統 丨（GPS)處理器30的該微處理器354的GPS觀測量和來自該高 度測量裝置20的高度測量值，並給出高精度的導航信息， 包括三維位置、三維速度和三維姿態；這些導航數據由該 ! INS處理裔41輸出到该輸入/輸出接口 50 ’其他機載航空電 I子系統可從該輸入/輸出接口 50獲取導航數據；如前所 !述，速度和加速度信息反饋給該全球定位系統（GPS)處理 I器3 0的微處理器3 5 4來輔助G P S衛星碼和載波相位跟縱。 ; 請續參閱第4a-l圖所示，在第（2)步驟來自該慣性測

j量單元10的慣性觀測量送給INS處理器41 ’並在該INS處硬 器4 1中進行慣性導航處理；這些慣性觀測量是機體轉動迷 率和比力。 ! 請參閱第4a-l圖所示，在第（3)步驟中來自該高度測 !量裝置2 0的高度測量值送給該卡爾曼濾波器4 3，並由卡爾

第23頁 531657 五、發明說明（19) 曼濾波器執行組合遽波處理。 請續參閱第乜―1圖所示，在第（4)步驟中該八 系統（GPS)處理器3〇的該微處理器3 54輪出偽距、定伋 移、GPS衛星星曆表、以及大氣參數給卡爾曼濾波。、卜勒頻 卡爾曼濾波器中來自该彳貝性導航系統處理器4 1、在 度測量裝置2 0的咼度信息、來自栽波相位整周模:自邊高 塊42和該全球定位系統（GPS)處理器30的該微處理解模 數據被融合，從而^出丨貝性V航系統的位置誤差、°、5 4的 差和姿態誤差；在第（5)步驟該慣性導航系統處理1度^ 理慣性測量值，它們是機體角數率和比力，和來 ' 处 曼濾波器4 3的位置誤差、速度誤差和姿態誤差，從爾 校正後的導航解；導航解包括三維位置、三維速度 = 姿態；這些導航數據-方面送入該卡爾曼遽波器用^同 GPS數據和高度信息作組合，另〜方面在第（9 )步驟送认 該輸入/輸出接口50，以便於其他機載航空電子系統獲& 導航信息。 雷達高度計是一種高度測量裝置，它測量由地面到飛 行體的高度，此高度被稱作為地面高度；第切_2圖說明利 用一雷達高度計21與GPS/IMU作組合導航定位處理，在第 (4)步驟中，由該雷達高度計2 1測得的地面高度數據送給 一數據融合模塊4 4，一地形數據庫4 5接收來自該丨Ν s處理 器4 1的當前飛行體的位置數據，並導出對應當前位置的地 面距離海平面的高度，該高度稱為地形高度，該地形高度 也送給該數據融合模塊4 4。

第24頁 531657 五、發明說明（2θ) 一^ η 該數據融合模塊44從該雷達高度計21接收飛行體距離 地面的地面高度數據，從該地形數據庫45接收當前飛行體 位置對應的地面距離海平面的地形高度數據，並作相加運 算得出飛行體距離海平面的高度，該高度值稱為海平面高 度，該海平面高度由該數據融合模塊44送給該卡爾曼濾波 器43 ’並用於組建該卡爾曼濾波器的觀測量方程。 請參閱第4a~2圖所示，在第（4)步驟該全球定位系統 ’ i (GPS)處理器30的該微處理器354輸出偽距、都卜勒頻移、 、 GPS衛星星曆表，以及大氣參數給該卡爾曼濾波器，在該 卡爾曼濾波器中來自該慣性導航系統處理器4 1、來自該數 據融合模塊4 4的高度信息、來自該載波相位整周模糊求解| 模塊42和該全球定位系統（Gps)處理器3〇的該微處理器354 | !的數據被融合，從而導出慣性導航系統的位置誤差、速度| 丨誤差和姿悲誤差；在第（5)步驟該慣性導航系統處理器41 | |處理仏性測量值，它們是機體角數率和比力，和來自該卡丨 |爾曼遽波器43的位置誤差、速度誤差和姿態誤差，從而導| 出权正後的導航解，導航解包括三維位置、三維速度和三 維姿態’這些導航數據一方面送入該卡爾曼濾波器43用於 同GPS數據和高度信息作組合，另一方面在第（9)步驟送給

!該輸入/輸出接口 5 0，以便於其他機載航空電子系統獲得 T 導航信息。 請續參閱第5圖所示，該慣性導航系統處理器4 1包含 一 IMU輸入/輸出接口 41 1、一 IMU誤差補償模塊412、一坐 ’ 標轉換計算模塊4 1 3、一姿態位置速度計算模塊41 4、一轉

第25頁 531657 五、發明說明（21) 換矩陣計算模塊4 1 5和一地球和運載體轉動速率計算模塊 41 6 〇 請參閱第5圖所示，在第（5)步該IMU輸入/輸出接口 411接收來自IMU10的機體角速率和比力信號，並將它們轉 換為數字數據，這些數據實際上帶有測量誤差，而帶有測 量誤差的這些數據送給該IMlJ誤差補償模塊412，該IMU誤 差補償模塊412同時從該卡爾曼濾波器43接收傳感器誤差 ^計，並對IMU數據作ΙΜϋ傳感器誤差消除處理；校正後的 1*貝f生數據被送給坐標轉換計算模塊4 1 3和該轉換矩陣計算 杈塊415，更深而言之，校正後的機體轉動角速率送給該 ΐίϊΠ算模塊415，而校正後的比力送給該坐標轉換 汁异模塊4 1 3。 請續參閱第5圖所示，在第（5 )步驟中 模塊4 1 5接收爽白T 兰$於此&外丫。茨轉換矩陣δ十异 和來自地Hi 償模塊412的機體轉動角速率 以和連載體轉動速率計算模仙 肢速率，亚進行轉換矩陣的計算， 表秤％仃 標轉換計算模塊413和該姿態心二被送給該坐 |用在該轉換矩它被 剛體的轉動；從機體系到導航系的v以方中程以為描述 其中 5 ατ - γ 。曰 h q "[% % Ub疋的反對稱矩陣，a 疋四元素參量的四個分量； 由陀螺儀測得，它是在機體系 531657 五、發明說明（22) 中機體相對於慣性系的旋轉速率向量。 Ο ~ωΐχ 6,v ~(Dbz [Ω,] bz Ο

Kc，〜，叫J7

Jby ^bx ο 是在導航系中導航坐 Ωη是向量的反對稱矩陣， 標系到慣性系的旋轉速率向量 [Ω ] ny [〜r，〜，d 如杲導航坐標系是北東下（NED)坐標系，則有：

(ωβ +/L)cosZ

~L

-(c〇i： - λ) sin L 其中，是地球旋轉速率，L是地理緯度’ λ是地理 經度 請參閱第5圖所示，在第（5 )步驟中該坐標轉換計算模 塊41 3從該I MU誤差補償模塊41 2接收比力，從該轉換矩陣

第27頁 531657 丨五、發明說明（23) 計算模塊41 5接收轉換矩陣來進行坐桿 換到由轉換矩陣表徵的坐標系中的比;^將轉 度計算模塊4 1 4。 z文悲位置速 請續參閱第5圖所示，在第（5)步驟 :算模塊414從該坐標轉換計算模塊413接收度 力、從該轉換矩陣計算模塊41 5接收轉換矩陣，並進行姿 態位置和速度更新，描述在地球表面或近地面的質點運動 的方程為： 、 V{t) ^ α- {2ωιβ ^ω2η)χν~ ω；β χ χ r I 其中：&和¥是導航坐標系中飛行體相對地球的加速度 |和速度，是地球旋轉向量，Wen是導航系相對於地球的 角速率，r是飛行體相對地球中心的位置向量。 因為加速度計不區分飛行體加速度和質量之間的引 力’由加速度計測量得到的比力可以表達為： f 二a-g(r) 其中’ g (r)是地球引力和飛行體離心力之和；由此 有

第28頁 531657 五、發明說明（24) 其中， ωβ cosL XcosL ①ί二 0 ，⑺⑺二 -L \ -A sin L — 一 飛行體速度由下式更新 yn +Myn ^gn 其中，Q”是從機體系到導航系的方向餘弦矩陣，並 有：

Vn = Ve ，fb 二 ~fb: fby ，gn 二 r 0' 〇 一、」 _fbz 一

-(2ωβ + λ) smL L

Μ 二(2% + i)sin L —L 〇 (2^e + A) cosZ. -(2ωβ + i)cosZ 0 由WGS-84橢球體的一般公式可以導出 ~ [1 ~ 2(1 + y*-f/^7^) —l· (―/?/ — f')sin~ Z] a 2 五 、發明說明（25) {m - ω^α2b/GM) 率 數 用於地理坐標更新的差分方程為： Z 二 v 义

V

+ h {Rn +h)c〇sL ,h 其中，rm是經線方向上曲率半徑，Rn是主垂線半徑。 請，，考w圖所示’在扣）步驟中由該卡爾曼遽波 认“ Α 弟禮方法疋將該卡爾曼濾波器4 3得到 誤差送給該姿態位置速度計算模塊414來進行直接 卡ΐ t ’第一種方法是在進行姿態計算之前，將由該 415進行姿態校正。 差运給該轉換矩陣計算模塊 上、ti5圖所示，在第（4)步驟中校正後的慣性解送給該 估爾1 :波器43 ’用它們來生成該卡爾曼濾波器的觀測 閱第“―1—2圖所示，在第⑺步驟中校正 助嫁觸衛星載波相位Λ相位整周模糊求解模塊42來輔 戟皮相俊整周模糊值，參考第4a-1，4a-2

第30頁 53!657 五、發明說明（26) 圖’在第（6 )步驟校正後的速声 系统〔Γ D C、也 又和加速度送給該全球定位 丁、、MGPS)處理器30的該微處踩哭十从 长疋位 的載波相位和碼跟蹤，請參/^3054/_„輔助GPS衛星信號 姿態、位置和速度信息也送圖入戶二在第(9)步驟中 其他機載航* + +系# #也輸輸出接口50 ,它為 传：戰航i包子糸統提供導航數據。 請續參閱第5圖所示，在第f R、半_ 士丄 度計算模塊414得到的姿態、位置和少速广关J亥妾广位置速 载體轉動速率計算模塊416來 球^ 2 = ^地球和運 旋轉速率，這此速率1##^/^球靛轉速率和飛行體 手、二逑羊值再廷給該轉換矩陣計算模塊415。 在統計特性已知情況下，爷晶 σ 、 紐 ^ ^ σΛ卡爾又遽波|§產生最佳 ?; 些最佳解是無偏的，在線性無偏估計值中它：：有 取小的方i，估計的質量只有告 ：：Τ匕們具有 /曰y丨， 3田數子模型正確的情汉下能 付到保證，任何與模型之間的 變壞。 夭配都了此導致濾波器性能 在增強的GPS/ΙΜϋ組合定位與導航系統 _

該卡爾曼濾波器可用於位置姿離管， 穩H 波〒的％疋，以保證可在不同的動態環境下工作，如 ΐ動：壞，改變了，或者是傳感器發生故障，例如GPS衛 星仏號故卩早或慣性傳感器故障，爷滹 ㈣㈣。 n皮以檢測、識別和 一穩健濾波器能對一組過程和觀測模型提供次佳解， 純粹的該卡爾曼濾波器不夠穩健是因為它是通過精碉後對 某一特殊的過程和觀測模型提供最佳解，濾波哭性 是用於保證從誤差協方差預測的導航性能接近真奋估叶的 531657 五、發明說明（27) 誤差特性；另外，濾'波器的發散通常由 模型或傳感器故障引起。 夂匕的過耘或硯測 哭…t ί日f用殘差監視的方法來獲得穩健的該卡爾曼遽波 、:旦;x f 用於GPS原始數據’慣性數據和來自該高V j 1裝置的咼度測量值的融合；當合適的冗余可用 X 差監視方法的一個好處就是當> ^ '殘 的統計特性分布是已知的=波差序列 布試驗於觀測量殘差來生成故障檢測方案，： 性可用於評價濾波器的調譜，當檢 調：=寺 的大小，第6圖給出了一句入总呈y、日欣叶π正協方差 爾曼濾波器的實現。 3殘差-視功能的穩健的該卡 從☆所不’在第⑷步驟中—gps誤差補償模塊437 括ϋ ϊϊί統（奶）處理器30搜集GPS原始觀測量，包 439接收位置_目/W和都卜勒頻移，從狀態向量更新模塊 GPS原於數攄ί f正來進行奶誤差補償，校正後的 G b原始數據迗給預處理模塊435。 435 ^續全參破閱圖所示，在第⑷步驟中該預處理模塊 .,^王弋位系統（GPS)處理器30接收GPS衛星星谔 包括偽距、載波相：ΛΛ 校正的GPS原始數據， 理器41接收ί:Γ:Γ 頻移’從該慣性導航系統處 時將它和前述狀亚進行狀態傳輸矩陣的計算，同 處理模塊435計/觀向^矩^一協方^傳播模塊432;該預 & %、、ai w ^硯測矩陣，並根據計算得到的觀測矩陣 “、型计异當前觀測向量，觀測矩陣及當前觀測向量

第32頁 531657 五'發明說明（28) 送給一觀測量殘差計算模塊438。 清參閱第5圖所示，在第（4)步驟中一狀態向量預測模 = 436從該預處理模塊435接收狀態傳輸矩陣和前一時刻狀 態向量，並進行當前狀態預測，預測的當前向量送給該觀 測量殘差計算模塊438。 » * 1、’、夷參閱第5圖所示’在第（4)步驟中該觀測量殘差計 ^ =塊=8從該狀態向量預測模塊336接收預測的當前狀態 二里，從該預處理模塊4 3 5接收觀測矩陣和當前觀測向 里，通過預測的當前狀態向量與觀測矩陣 二=目減得到觀測量殘差，觀測量殘差送 权鬼431和该狀態向量更新模塊439。 4Ή、Λ參Λ第5圖所示’在第⑷步驟中該殘差監視模塊 #方^判別，判別的準則是觀測量殘差的平方被殘 差的平』：Ϊ的值是否大於一給定門限值’如果觀測量殘 的觀、、^ Ϊ 方差相除後的值大於該給定門限，則當前 = ;導致Λ卡爾曼遽波器的發散；如果這樣，該 前的觀測量，4如31/_异(一旦新的系統協方差’或拒絕接收當 值小於吱认— 硯/則里殘差的平方被殘差方差相除後的 而不需；Ϊ : I二則當前的觀測量應用於卡爾曼濾波， 協方差送給；協方差來獲取當前的導航解，系統 社分a 4協方差傳播模塊432。 塊432從該殘赫於、°所不，在第（4)步驟中該協方差傳播模 模塊435接收壯模塊431接收系統協方差，從該預處理 怨傳輪矩陣，以及前一時刻的估計誤差的

第33頁 531657 五、發明說明（29) 協方差’並計算當前估計誤差的協方差；計算得到的估計 誤差2協方差送給最優增益計算模塊433。 請續參閱第5圖所示，在第（4 )步驟中最優增益計算模 塊4ϋ從該協方差傳播模塊432接收估計誤差的協方差，並 計异最優增益；該最優增益送給一協方差更新模塊434， 同時也运給該狀態向量更新模塊439，該協方差更新模塊 4 34更新估計誤差的協方差，並將它送給協方差傳播模塊 432 ° | 睛參閱第5圖所示，在第（4)步驟中該狀態向量更新模 i =439從該最優增益計算模塊433接收最優增益，從該觀測， =f差計算模塊438接收觀測量殘差，並計算狀態向量的 i 估片值匕們包括位置、速度和姿態誤差，這些誤差 i值运給該GPS誤差補償模塊437和該慣性導航系統處理器 丨 丨 41。 | 一應用GPS載波相位觀測量可以獲得比應用偽距觀測量| |更高的定位精度，這是因為GPSL1廣播頻率為 i I 1575·42ΜΗζ，其對應的載波波長為ig厘米，然而[/A碼的丨 | 一個碼片長約3 0 0米，且高精度的GPS載波相位定位是基於丨 !載波相位模糊值已知的前提下，載波相位模糊值依賴於丄 GPS接收機和GPS衛星，在理想的假設下，即沒有觀測誤 差二，，機和衛星的位置都精確已知，則該相位模糊可以 $ —簡單的算術方法得到，因為各種誤差的存在，如衛星 星曆表誤差、衛星鐘偏差、大氣傳播延遲誤差、多路徑效 -應、接收機鐘差、以及接收機噪聲，不可能得到精確的從

531657 五、發明說明 G,Pt接〜收機到衛星的精確幾何距離 該不精確幾何距離叫 做偽距 I辅助载波相位模糊度求 來自校正過的慣性導航解的精確和周跳檢測的好處在於 可用於辅助確定初始的整周模^行體的位置和速度信息 性導航系統辅助信號跟蹤加強了^搜索空間；另外，慣 的能力’這樣就減少了信號失鎖：：：：定GPS衛星信號 請續參閱第油_2圖所* 1 ^發能性。 位整周模糊求解模塊42從該慣4 步= 處理器354接收載波相位和都卜 =θ器30的該微 晶、套★以Q分 丨Γ初頻移親測量，從該卡爾 又濾波器43接收協方差矩陣，來求解Gps衛星信號敫 糊值；在整周模糊值確定後，它皮 i。、 推一半孩古「PC ^ 匕被达給该卡爾曼濾波器來 進γ誕问GpS硯測量精度和組合導航解的精度。 IMU輔助GPS衛星信號載波相位整周模糊解由第7圖仏 出，它包括一幾何距離計算模塊421、最小平方調整_ 422、一衛星鐘模型423、一電離層模型424、一對流層模 型425、一衛生預測模塊426、和一收索空間破定模塊、 GPS衛星信號載波相位模糊度的一個基本特徵是當保 持衛星信號跟蹤不間斷的情況下，它不隨時間改變；曰沽 相位觀測量模型為： / 531657 五、發明說明（31) 其中，Φ是測得的載波相位；λ是信號波長 收機到衛星之間的真實幾何距離；f是信號頻率； △ 5 = h是鐘誤差；心是衛星鐘偏移；心是接收機 鐘誤差；N是載波相位整周模糊值；deph是星曆表誤差引入 的距離測量誤差；d_是由電離層引入的傳播誤差；d⑽是 由對流層引入的傳播誤差；ε是相位測量噪聲。 # $ * ΐ頻數據可用時，雙頻載波相位觀測量可用來消除 、、/所的包離層誤差，而且ΙΜϋ輔助載波相位模糊解用 :^ =頻載波相位觀測量形成的寬道信號；寬道信號可以 表達為： Φ 〇Ll ~ 其中 Φ L1 是L1通道載波相位觀測值；φ。是12通道載 波相位觀測值；相應的寬道信號頻率和相“ς值 L1 L2

Jw - Ν L1 'L2 帶來度Γ的問題更進-步由於衛星信號失鎖 必須檢測出來；：：：化有為導航解’周跳 於樹木、建筚物、於ΐ’有二種原因可來周跳’第-是由 杂物“朱和山脈等對衛星信號的遮擋，這是

531657 五、發明說明（32) 最為常見的一種引起周跳的原因； 一 是低的信噪比（SNR)，它主要來自& &二種W起周跳的原囚 徑影響、讀高動：運動離層，境、多； 跳的原因是接收機振蕩器電路；=角啦弟三種引起周 用於周跳的檢測和修復。 本發明中，IMU輔助也 請參閱第6圖所示，在第（7)步 426從兮八技令#么 乂驟中該衛星預測模塊 4Zb仗該全球疋位系統（Gps)處理 星曆表數據，並進行衛星位置計ί30接收可見GPS衛星的 仏心廷給遠幾何距離計算模塊仍 罝 4 2 1同時捲收來白兮# α/ 落成何距離计异模塊 位置二Λ 糸統處理器41的飛行體精確 :ΐ:链 計算從GPS接收機到衛星之間的幾何距 碼跟蹤環路導出的偽=ί (G ㈣的 ^ τ出的偽距不一樣，該計算出來的幾何距離送 入該取小平方調整模塊422。 參閱第7圖所示，在第⑺步驟中該對流層模型 一 °亥王球定位系統（GPS)處理器得到時間標誌；並應用 士 £的對流層傳播延遲模型計算GPS衛星信號的對流層 叶^ 计鼻出來的對流層時延送給該最小平方調整模塊 4： 2 2 ° up明參閱第7圖所示，在第（7 )步驟中該電離層模型4 2 4 伙°亥全球定位系統（GPS)處理器得到時間標誌和電離層參 數並由一内置的電離層傳播延遲模型計算由電離層引入 的^延’計异出來的電離層時延送給該最小平方調整模塊 422 〇

第37頁 531657 五、發明說明（33) 請續參閱第7圖所示，在第（7)步驟中該衛星鐘模型 423接收GPS衛星鐘參數；並計算衛星鐘校正量，計算出來 的衛星鐘校正量送給該最小平方調整模塊4 2 2。 請參閱第7圖所示，在第（7)步驟中該收索空間確定模 塊427從該卡爾曼濾波器43接收觀測向量的協方差矩陣， 基於該協方差矩陣，該收索空間確定模塊427導出觀測量 誤差’並確疋G P S树生載波相位整周模糊搜索空間，該載 |波相位整周模糊搜索空間也送給該最小平方調整模塊 ! 422。 、

I | 請續參閱第7圖所示，在第（7)步驟中該最小平方碉整 模塊422從該幾何距離計算模塊42 }接收從飛行體到Gps衛 星之間的幾何距離、從該對流層模型425接收對流層時 丨延、從該電離層模型424接收電離層時延、從該衛^鐘模 丨，423接收衛星鐘校正量，由此計算初始的搜索原點；該 I取小平方調整模塊422也從該收索空間確定模塊427接收載 |波相位整周模糊搜索空間，然後用一標準的最小平方 |來確定這個載波相位模糊值。 °工 ^如上所述為本發明的第一優選實現方案，它可稱為今 高度測量裝置輔助全耦合GPS/IMU導航定位方法和系統、、'，〃 | 該導航定位方法和系統用一卡爾曼濾波器融合GPS偽距、擎 载波相位、都卜勒頻、慣性觀測量和高度觀測值；該 方式要求GPS接收機提供載波相位觀測值，由此增加了口 i全球定位系統（GPS)處理器的複雜性和成纟；本發 = 一優選實現方案將校正後的慣性導航解即速度和加速度反

第38頁 531657 五、發明說明（34) ~------- 饋給該全球定位系統（GPS)處理器來輔助Gps衛星信號的碼 和載波相位跟蹤，校正後的慣性導航同時也用於輔助 衛^信號整周模糊度求解，來自該高度測量裝置的高度觀 f蓋用於提问系統的垂直導航精度，該雷達高度計是一種 高_度測量裝置·，本發明的第一優選實現方案用一數據融合 核塊和一地形數據庫來處理由該雷達高度計得來的地面高 度數據’並由該數據融合模塊輸出海平面高度數據給該卡 爾曼濾波器’該卡爾曼濾波器對該海平面高度和GPS觀測 |量、INS導航解作組合濾波處理。

I | 南度觀測量值也可用於緊密型GPS/IMU組合系統以提 · |南其垂直導航精度，本發明的第二優選實現方案用該卡爾I !曼濾波器融合Gps偽距和都卜勒頻率、慣性觀測量和來自| 該高度測量裝置的高度觀測量，與第一優選實現方案不同| I的疋’在本貫現方案中GPS載波相位沒有用於組合導航處 理。 請參閱第1、2b、3b、4b-1、4b-2、5、6圖所示，說 丨明本發明的第二優選實現方案；本發明的第二優選實現方 丨案包含如下步驟： 丨 ! (1)執行GPS處理，從該全球定位系統（GPS)處理器30 ; |接收GPS觀測量，它們是偽距和都卜勒頻移，並將它們送 f 給該中心導航處理器4〇。 | ! ( 2 )從該慣性測量單元1 0接收慣性觀測量，它們是機 丨體轉動角速率和比力，並將它們注入該中心導航處理器40 的該I N S處理器41。 531657 五、發明說明（35) ~一~—— (3)從該高度測量裝置2〇接收高度觀測量，並將它 、'Ό違中心導航處理器4 0。 (j)在該卡爾曼濾波器43中融合該INS處理器41的輪 问度觀測量值和GPs偽距、都卜勒頻率。 ^ (5)執行1 NS處理，並將該卡爾曼濾波器43的輸出反鐘 、巧該INS處理器41來校正INS導航解。 (6)將導航數據從該INS處理器41送給該輸入/輪出接

獲取導航數據。 U 在第（5 )步驟之後，可附加如下一步： (58/)將該iNS處理器41的速度和加速度輸出注入該全 球定位糸統（GPS)處理器3〇的該微處理器3 54，並執行Gps 爾生彳§號的碼輔助跟縱。 口月芩閱第2b、3b、4b-1和4b-2圖所示，在第（丨）步驟 反込貝現方案，除了不進行GPS載波相位 跟蹤和速度加速度辅助Gps載波相位跟蹤 優選實現方案相同的工作，該中心導航處理器40只從該全 球定位糸統（GPS )處理莠q n掉故爲扣i 土 仗乂王 有GPS載波相位。"3°接收偽距和都卜勒信息，而沒 請續參閱第2b圖所$ r η ^ 揀突35外、該八域…斤了在弟（1)步驟中除了該信號處 二料相哭ϋ王ΤΡ疋位系統天線31、該前置放大電路32、 伯^和A/dH/哭技案相同的工作；該信號處理器35從1F 採樣 # 一接收數位化了的GPS數據，解調出調劑在

531657 五、發明說明（36) -'- GPS信號上的導航數據，如GPS衛星星曆表、大氣參數 以 星鐘參數以及時間信息，該信號處理器3 5同時溻彡1 ^、俯 必攸14 4b數 位化的G P S數據中導出偽距和都卜勒頻率，該偽距和都 勒頻率送給該中心導航處理器4 〇 ;在第（5 a)步驟中，P卜> 號處理器3 5從該中心導航處理器4 0接收速度和加括由^仏 /7U迷度用於 輔助GPS衛星信號碼跟蹤。 ' 請參閱第3b圖所示，在第（1 )步驟中，GPS偽距由Gps 碼及蹤環導出來，GPS碼跟蹤環路包括一相關器35 2、_累 加器3 5 3、一微處理器3 54、一碼NC0357、和一編螞器 ’、 3 5 6 ;都卜勒頻率由GPS衛星信號頻率跟蹤環路導 °， 率跟蹤環不同於第一優選實現方案的載波相位跟縱環，頻 率跟蹤環包括一都卜勒移去器3 5 1、一相關器3 5 2、—累力' 器3 53、一微處理器354和一載波“〇3 5 5，在頻率跟蹤環^ 微處理器3 5 4不執行載波相位檢測。 衣 請參閱第3b圖所示’在第（1)步驟中，該累加器353將 累加量（13和Q3)送給該微處理器354，然後將該累加哭353 清零，此即為信號分量的累加一倒空濾波，該微處理。。器 354執行碼跟蹤環路濾波、碼捕獲處理、碼鎮定檢測、°數 據恢復、以及偽距和都卜勒頻率處理；在（5a)步驟中， 該微處理器354從該中心導航處理器4〇接收速度和加速产 ，息’並進行速度加速度輔助的碼跟蹤環路濾波、碼捕又獲 J理、碼鎮定檢測、數據恢復、以及碼和都卜勒頻率處又 請續參閱第3b圖所示，在第（1)步驟中該微處理器354

531657 五、發明說明（37) 輸出GPS偽距和都卜勒頻移給該中心導航處理器40。 請參閱第4b-1和4b-2圖所示，在第（2)步驟中該慣性 測量單元1 0輸出機體轉動速率和比力等慣性觀測量給該 I NS處理器4 1，在第（3 )步驟中，該高度測量裝置測量飛行 體高度，並將該高度數據送給該中心導航處理器4 0。 請續參閱第4b-l圖所示，在第（4)步驟中該全球定位 系統（GPS )處理器3 0的該微處理器3 5 4輸出偽距、都卜勒頻 移、GPS衛星星曆表、以及大氣參數給該卡爾曼濾波器； 在該卡爾曼濾波器中來自該慣性導航系統處理器4 1、來自 I該高度測量裝置20的高度信息、和來自該全球定位系統 | (GPS)處理器30的該微處理器354的數據被融合，從而導出 慣性導航系統的位置誤差、速度誤差和姿態誤差；在第 i (5)步驟中該慣性導航系統處理器41處理慣性測量值，它 !們是機體角數率和比力，和來自該卡爾曼渡波器43的位置 I誤差、速度誤差和姿態誤差，從而導出校正後的導航解， 丨導航解包括三維位置、三維速度和三維姿態，這些導航數 據一方面送入該卡爾曼濾波器43用於同GPS數據和高度信 息作組合，另一方面在第（6)步驟中送給該輸入/輸出接口 5 0，以便於其他機載航空電子系統獲得導航信息。 請參閱第4b-2圖所示，一雷達高度計21是一種該高度 |測量裝置，它測量由地面到飛行體的高度，此高度被稱作 !為地面高度，如第4b-2圖說明利用一雷達高度計與 GPS/IMU作組合導航定位處理，在第（4)步驟中，由該雷達 高度計2 1測得的地面高度數據送給一數據融合模塊44，一

第42頁 531657 五、發明說明（38) 地形數據庫4 5接收來自該I NS處理器4 1的當前飛行體的位 置數據，並導出對應當前位置的地形高度，該地形高度也 送給該數據融合模塊44，該數據融合模塊44由此導出載體 的海平面高度，並將它送給該卡爾曼濾波器4 3，並用於組 建卡爾曼濾波器的觀測量方程。 請續參閱第4b-2圖所示，在第（4)步驟中該全球定位 -系統（GPS )處理器3 0的該微處理器3 5 4輸出偽距、都卜勒頻 _ 移、GPS衛星星曆表、以及大氣參數給該卡爾曼濾波器； | 在該卡爾曼濾波器中來自該慣性導航系統處理器41，來自 | 該數據融合模塊44的高度信息、和來自該全球定位系統 參 | (GPS)處理器30的該微處理器354的數據被融合，從而導出 |慣性導航系統的位置誤差、速度誤差和姿態誤差；在第 (5)步驟中該慣性導航系統處理器4 1處理慣性測量值，它 們是機體角數率和比力，和來自該卡爾曼濾波器4 3的位置 |誤差、速度誤差和姿態誤差，從而導出校正後的導航解； |導航解包括三維位置、三維位置和三維姿態。這些導航數 丨

I據一方面送入卡爾曼濾波43用於同GPS數據和高度信息作 I

i組合，另一方面在第（6)步驟送給該輸入/輸出接口 50，以 I I ί |便於其他機載航空電子系統獲得導航信息。 1 請參閱第5圖所示，在第（5 )步驟中本發明的第二優選 Φ

I 實現方案中該INS處理器41執行與第一優選實現方案中同 ! I樣的工作。 請續參閱第6圖所示，在第（4)步驟中除了該GPS誤差 補償模塊437外，第二優選實現方案中的該卡爾曼濾波器

第43頁 531657 五、發明說明（39) 43執行與第一優選實現方案中同樣的工； 一 現方案中該GPS誤差補償模塊437從該全球定二優選實 置^丰載波相位數據’從該狀態向量更新模塊439接收位 置和速度校正量，並進行GPS誤差補償，校正 < 數據即偽距和都卜勒頻率送給該預處理模塊435。、 ，、始 、增強型該高度測量裝置輔助緊密型(}1^/11^1]組合 方法和系統用一卡爾曼濾波器處理GPS偽距和都卜二、 ^、慣性導航解和來自-高度測量裝置的高度觀測量，並 導出。慣性導航系統的導航參數誤差的最優估計、慣性傳感 器誤差的最優估計、GPS接收設備時鐘偏移的最優估計，〜 這些誤差估計用於校正慣性導航系統來導出組合導航解， 引入南度觀測量是用於改善垂直導航精度，校正後的慣性 速度和加速度反饋回該全球定位系統（GPS)處理器來輔助 GPS衛星信號的碼跟蹤，第二優選實現方案提供的這種組 合模式不要求GPS載波相位，故而降低了系統複雜度和成 本’由於緊密型GPS/IMU組合沒有利用高精度的載波相位 觀測量，因而它的導航精度低於全耦合GPS/IMU組合系統 的導航精度。 鬆散型G P S / I M U組合系統是一種最簡單的g p s同I μ U進 行組合的方式，它將GPS接收機導出的位置和速度信息作 為該卡爾曼濾波器的觀測量，這種組合模式不要求高速的 組合濾波處理器，對GPS接收設備的要求也不高，因而具 有低成本的優點。

531657 五、發明說明（40) 高度觀測量也可用於鬆散型GPS/IMU組合系統以提高 其垂直導航精度’將高度觀測量引入鬆散型GPS/IMU組合 系統即為本發明的第三優選實現方案，該第三優選實現方 案用一卡爾曼濾波器融合GPS位置和速度，慣性導航解以 及來自該南度测里裝置的南度觀測量，與前兩種實現方案 相區別的是：在第三優選實現方案中沒有外來信息輔助 GPS衛星信號碼跟蹤和載波相位跟蹤，而且該實現方案利 用GPS位置和速度建立該卡爾曼濾波器的觀測量方程，而 丨不是偽距、都卜勒頻率和載波相位。 請參閱第1、2c、3c、4c-l、4c-2、5和6圖所示，係 ^ 本發明的第三優選實現方案；該第三優選實現方案包括如| 丨下步驟： 丨 I (1 )執行GPS處理並從該全球定位系統（GPS)處理器3〇 I接收GPS位置和速度信息，並將它們送給該中心導航處理 丨 I 器 40。 | (2)從該慣性測量單元ίο接收慣性觀測量，它們是機 |體的轉動速率和比力，並將它們送給該中心導航處理器 的慣性導航該I N S處理器4 1。

(3 )從該高度測量裝置2 〇接收高度觀測量，並將它 給該中心導航處理器4 〇。 、 (4) 在該卡爾曼濾波器43中融合該INS處理器41的輪 出、高度觀測量、以及GPS位置和速度。 則 (5) 執行INS處理，並將該卡爾曼濾波器43的輸出 給該INS處理器41來校正ins導航解。 貝

531657 五、發明說明（41) (6)將導航數據由該INS處理器41輸出到該輸入/輸出 接口 5 0 ’其他機載航空電子系統可從該輸入/輸出接口 5 〇 獲取導航解。

t考第2 c圖’在第u )步驟中除了該信號處理器3 5 外、該全球定位系統天線31、該前置放大電路32、該下變 頻is 33、IF採樣和A/E)轉換器34和該振蕩器電路36執行與 第二和第二優選實現方案一樣的工作；該信號處理器35從 IF採樣和A/D轉換器34接收數位化的Gps數據，並從中解調 I出GPS衛星星曆數據、大氣參數、衛星時鐘參數、以及時 I間信息’該信號處理器35同時還從這些來自IF採樣和A/D |轉換34的GPS數據中導出偽距和都卜勒頻率，該偽距和 |都卜勒頻率送給全球定位系統GPS導航處理器，該信號處 !理器35不執行速度加速度輔助的GPS碼和載波相位跟縱。 i請參閱第2c圖所示，在第（1)步驟中該全球定位系統Gps導 |航處理器37用作求解飛行體的位置和速度，該全球定位系 |統GPS導航處理器37從該信號處理器35接收偽距和都卜勒' |頻率’並應用卡爾曼濾波方法或最小二乘法求解飛行體位 !置和速度’該位置和速度數據送給該中心導航處理器4〇。

|請續參閱第3c圖所示，在第（1)步驟中該都卜勒移去器 351、該相關器352、該累加器3 5 3、該載波NC〇3 5 5、該碼 產生器356和該碼NC0357做與第一和第二優選實現方案同 樣的工作，唯有該微處理器354做不一樣事，該微處理器 3 5 4執行碼跟蹤環路濾波、碼捕獲處理、碼饋住檢測、數 據恢復以及偽距和都卜勒頻率處理，它不接收外來的速度

第46頁 531657 五 '發明說明（42) 石度信息，也不執行外來速度加速度輔助的GPS信號 -和載波相位跟蹤，由該微處理器3 54導 〔 勒頻率送給該全球定位系統GPS導航處偽距和都卜 請續參閱第3c圖所示，在第（1)步驟申， 將累加量和Q3)送給該微處理器354，然後=^3 3 53清零，此即為俨狹八旦沾罗 、以累加口口、 突I 虎刀里的累加一倒空濾波，該微處理 : 3 54執_艮蹤環路據波、碼捕獲處理、碼鎮定檢：、 :恢復、以及偽距和都卜勒頻率處理，該微處理哭354 =速度加$度辅助的碼跟縱if路遽波’ I出的偽距和 都卜勒頻率达給該全球定位系統GPS導航處理器37。·..· 凊參閱第3c圖所示，在第半踩^ ΓΡς道p ♦神吳q7认 弟U )步驟中该全球定位系統 :sv航處理輪出GPS位置和速度給該中心導航處理器 請續參閱第4c~~l圖和第4r-9固〜- 該慣性測量單元10輪出機體轉動爷：’在第⑴步驟中 A Μ #! 轉動速率和比力等慣性觀測量 給遠INS /处理|§ 41 ，在第（3)步驟中，誃古 飛行體高度，並將該高度數櫨、> ° 〇又’、里、測里 „ .= 據达給該中心導航處理器40。 ㉔爹閱第4c-!圖所示，在第（4)步驟 統GPS導航處理器37輸出位置％疮啟 王衣疋位糸 讯κ议罝和速度偽距、Gps衛星星 表、以及大氣參數給該卡爾晶、清 曰 + I自，It性I & έ Μ + 又濾波在該卡爾曼濾波器 中來自t貝〖生導航糸統處理器41、 據被融合，從而導出慣性導航广航處理器37的數 和姿態誤差、在第⑴步驟中/'Λ曾誤差m差 、」乂知中泫慣性導航系統處理器4丨處

第47頁

531657 五、發明說明（43) 理慣性測量值，它們是機體角數率和比力，和來自該卡爾 曼濾波器4 3的位置誤差、速度誤差和姿態誤差，從而導出 校正後的導航解，導航解包括三維位置、三維位置和三維 姿態，這些導航數據一方面送入該卡爾曼濾波器43用於同 G P S數據和高度信息作組合，另一方面在第（6 )步驟中送給 該輸入/輸出接口 5 〇，以便於其他機載航空電子系統獲得 導航信息。 請續參閱第4c-2圖所示，一雷達高度計21是一種該高 度測里裝置’它測量由地面到飛行體的高度，此高度被稱 作為地面高度，第4c-2圖說明利用一雷達高度計與 GfS/IMU作組合導航定位處理，在第（4)步驟中，由該雷達 冋度2 1測得的地面高度數據送給一數據融合模塊4 4，一 地形數據庫45接收來自該INS處理器41的當前飛行體的位 、，仏^ 並V出對應當前位置的地形高度，該地形高度也 运=4數，融合模塊44，該數據融合模塊44由此導出載體 ^ f平面向度’並將它送給該卡爾曼濾波器43，並用於組 建卡=曼濾波器的觀測量方程。 參，第4卜2圖所示’在第（4)步驟中該全球定位系 、、、、航處理器37輸出位置和速度、gpS衛星星歷表、以 及大氣參數认分^ 了 4卡爾曼濾波器；在該卡爾曼濾波器中來自 俨ίv，系統處理器41、來自該數據融合模塊44的高度 人从和來自5亥全球定位系統GPS導航處理器37數據被融 爷差·在^出丨M性導航系統的位置誤差、速度誤差和姿態 、 弟（5 )步驟中該慣性導航系統處理器41處理慣性

第48頁 531657 " ----- 〜---—____ 五、發明說明（44) 測量值，它們是機體角數率和比力，和來自該卡爾曼清 器43的位置誤差，速度誤差和姿態誤差，從而導出波 的導航解，導航解包括三維位置、三維位置和三維姿態後 這些導航數據一方面送入該卡爾曼濾波器43用於同G1s^， 據和高度信息作組合，另一方面在第（6 )步驟中送給該 入/輸出接口50，以便於其他機載航空電子系統獲得^ = 信息。 守飢 請參閱第5圖所示，在第（5 )步驟中本發明的第三 實現方案中該INS處理器41執行與第一和第二優撰每一 ·選 案中同樣的工作。 一定^現方 請續參閱第6圖所示，在第（4)步驟中除了該Gps誤 模塊437外，第三優選實現方案中的該卡爾曼濾波哭 3執仃與第一和第二優選實現方案中同樣的工作，在 =選實現方案中該GPS誤差補償模塊437從該全球定位ς GPS導航處理器37接收GPS位置和速度數據 . ^新4塊439接收位置和速度校正量’並進行Gps誤差 ^ ’校正後的GPS位置和速度數據送給該預處理模塊他。

增強型導航定位系統也可以與一手提 供年與+ # # mδ舌機組合以接 ::k -电活使用者的位置信息、，同時這個 J 過手提電話送給一俨自中心护制站，车扭不 L心了 M通 要1 T 控制站手^電話使用者的^ 置可以顯示在計算機的屏幕上；如果使 位 巧信息中心控制站則可以是機場，Gps接收‘私機， 提電話機組合來提供攜帶者的位置信息，者θ ^和手 位置信息。 及者疋運動體的

第49頁

Claims (1)

1. 531657 f l, tr_ 六、申請專利範圍 1. 一種增強型導航定位方法包括如下步驟： (a) 由一GPS處理器接收一組全球定位系統衛星信號， 並導出運動體的位置和速度信息，以及一組全球定位系統 原始觀測量，包括偽距，載波相位和多卜勒頻移； (b) 將該GPS原始觀測量由該GPS處理器送給一中心導 航處理器； (c )從一慣性測量裝置I M U接收一組慣性測量值，包括 運動體轉動角速率和比力； (d )將該慣性測量值由該I M U傳給該中心導航處理器的 |一慣性導航系統I NS處理器，並在該I NS處理器計算慣性導 I航解，包括該運動體的位置，速度，加速度，和姿態； S (e)從一高度測量裝置接收一運動體高度測量值； (f)在一卡爾曼濾波器中融合來自該INS處理器的該慣 '性導航解，來自該GPS處理器的該GPS原始觀測量，和來自 丨:該高度測量裝置的該運動體高度測量值，並導出一組INS :校正量和G P S校正量； 1 (g)將該INS校正量從該卡爾曼濾波器反饋給該INS處 ^理器，並校正該慣性導航解； 1 (h)將該運動體的該速度和加速度信息從該INS處理器 jut ► ^送給該GPS處理器的一微處理器，以輔助一組全球定位系 |統碼跟蹤環路和一組全球定位系統載波相位跟蹤環路用於 参捕獲和跟蹤該全球定位系統衛星信號，其中該GPS處理器 的該微處理器輸出該GPS原始觀測量，包括該偽距，該載 波相位，和該多卜勒頻移；

   第50頁 531657

   旦（^)將來自GPS處理器的該微處理器的該GPS原始觀測 、、壶 自〇 IN S處理器的該慣性導航解，和來自該卡爾@ ：器的該慣性校正量和該GPS校正量送入一冑波相位= 二^ Ϊ求解模塊來確定一組全球定位系統衛星信號载波相 位整周模糊值； ^收相 〜（])將該全球定位系統衛星信號載波相位整周模糊值 載波相位模糊求解模塊送給該卡爾曼濾波器 提南載體的導航精度； 逆步 (k)將該INS導航解從該INS處理器傳給一輸入/輪出 /〇)接口 ’從而為一個航電系統提供導航數據者。 复如專利申請範圍第1項所述之一增強型導航定位方法， 二中，該穩健卡爾曼濾波器包括一GPS誤差補償模塊用於 k忒GPS處理器接收該gps觀測量，包括該偽距，載波相位 和=卜勒頻率，從一狀態向量更新模塊接收位置和速度校 正里，亚執行GPS誤差補償以生成校正過的Gps原始數據， 亚將該GPS原始數據送給一預處理模塊，該預處理模塊從 忒GPS處理器接收GPS衛星星歷數據，從該GPS誤差補償模 塊接收該校正後的GPS原始數據，從該高度測量裝置接收 該運載體高度觀測量，從該INS處理器接收INS導航解，該 預處理模塊計算狀態傳輸矩陣，並將該狀態傳輸矩陣連同 該狀態向量送給一狀態向量預測模塊，該計算得到的狀態 傳輸矩陣也送給一協方差傳播模塊，該協方差傳播模塊根 據一計算得到的觀測矩陣和一觀測模型計算一觀測矩陣和 當前觀測向量，該觀測矩陣和該當前觀測向量送給一觀測

   2002.12.13. 051 531657 六、申請專利範圍 量殘差計算模塊，該狀態向量預測模塊從該預處理模塊接 收該狀態傳輸矩陣和該狀態向量，並對當前時刻的狀態進 行預測，該預測的當前狀態向量送給該觀測量殘差計算模 塊，該觀測量殘差計算模塊從該狀態向量預測模塊接收預 測的當前狀態向量，從該預處理模塊接收該測量矩陣和該 當前觀測向量，該觀測量殘差計算模塊通過從該當前觀測 向量減去該觀測矩陣與該當前狀態向量預測值的乘積來計 算觀測量殘差，該殘差監視模塊對從該觀測量殘差計算模 塊來的觀測量殘差進行判別，該協方差傳播模塊從該殘差 監視模塊接收系統過程的協方差，從預處理模塊接收狀態 傳輸矩陣和估計誤差的協方差來計算當前估計誤差的協方 差，該估計誤差的協方差送給一最優增益計算模塊，該最 優增益計算模塊從該協方差計算模塊接收該估計誤差的當 前協方差，並計算最優增益，該最優增益送給一協方差更 新模塊和該狀態向量更新模塊，該協方差更新模塊更新該 估計誤差的協方差，並將該估計誤差的協方差送給該協方 差傳播矩陣，該狀態向量更新模塊從該最優增益計算模塊 接收該最優增益，從該觀測量殘差計算模塊接收該觀測量 殘差，該狀態向量更新模塊計算當前狀態向量的估計值， 包括位置，速度和姿態誤差，該位置，速度和姿態誤差送 給該GPS誤差補償模塊和該INS處理器者。 3. —種增強型導航定位系統包括： 一全球定位系統GPS處理器用於提供與位置有關的信 息；

   第52頁 531657 六、申請專利範圍 一慣性測量單元I M U用於提供慣性觀測量，包括載體 的轉動角速率和比力； 一高度觀測量產生器用於提供載體高度觀測量；其中 所述高度觀測量產生器是一個高度測量裝置，用以提供平 均海平面以上的高度量測值； 一中心導航處理器，它與該GPS處理器，該IMU和該高 度觀測量產生器相連，並用於融合來自該GPS處理器的該 與位置有關的信息，來自該I M U的該慣性觀測量和來自該 高度觀測量產生器的該高度觀測量來產生載體的導航解， 包括位置，速度，和姿態； 一輸入/輸出（I / 0 )接口 ，它與該中心導航處理器相 連，用於輸出該導航解； 其中，所述全球定位系統處理器產生一組偽距，載杷 相位和多普勒頻移；其中所述中央航行處理器包括一個慣 性導航系統處理器，一個卡爾曼遽波器，一個載杷相位模 糊整數求解單元；其中，所述偽距，載杷相位和多普勒頻 移被傳送到中央航行處理器，所述平均海平面以上高度量 測值被傳送到卡爾曼濾波器，所述慣性量測值被傳送到慣 性導航系統處理器；其中所述慣性導航系統處理器的輸出 值，所述平均海平面以上高度量測值，所述偽距，載杷相 位和多普勒頻移在卡爾曼濾波器中被混合，而所述卡爾曼 濾波器的輸出則被反饋到所述慣性導航系統處理器中用來 校正從所述中央航行處理器輸出到所述I / 0界面的一個慣 性導航系統結果；其中所述慣性導航系統處理器為所述全

   第53頁 531657 六、申請專利範圍 球定位系統處理器提供速度和加速度數據以跟蹤全球定位 系統衛星信號；全球定位系統處理器，慣性導航系統處理 器，以及卡爾曼濾波器的輸出被發送到所述載杷相位模糊 整數求解模塊以求得全球定位系統載杷相位模糊整數；所 述載杷相位模糊整數求解模塊將所求得的載杷相位反饋到 卡爾曼濾波器以進一步提供定位精度；其中所述慣性導航 系統處理器將導航數據輸出到所述I / 〇界面； 所述全球定位處理器得微處理器輸出所述偽距，載杷 相位，多普勒頻移，全球定位系統星歷，以及大氣參數給 所述卡爾曼濾波器； 所述慣性導航系統處理器處理所述慣性量測值，包括 體角速度，外力，以及從卡爾曼濾波器得到的方位誤差， 速度誤差，高度誤差以推導經過校正的導航結果； 所述高度測量裝置將所述平均海平面以上的高度測量 值發送給卡爾曼渡波器者。 4.如專利申請範圍第3項所述之一增強型導航定位系統， 其中，該INS處理器包括一 IMU I/O接口、一 IMU誤差補償模 塊、一坐標轉換計算模塊、一姿態位置速度計算模塊、一 轉換矩陣計算模塊、和一地球和運動體轉動速率計算模 塊，其中，該IMUI/0接口從該IMU接收該機體轉動角速率 和比力信號，並處理轉化為數字數據，這些數字數據迭加 加有該慣性傳感器測量誤差，該受慣性傳感器測量誤差污 染的慣性測量值送給該I M U誤差補償模塊，其中，該I M U誤 差補償模塊接收來自該卡爾曼濾波器的傳感器誤差估計

   第54頁 531657 六、申請專利範圍 值，並執行I MU誤差校正，該校正後的慣性數據送給該坐 標轉換計算模塊和該轉換矩陣計算模塊，該比力送給該坐 標轉換計算模塊，該轉換矩陣計算模塊從該I MU誤差計算 模塊接收該機體角速率，從該地球和運載體轉動速率計算 模塊接收一地球和運載體轉動角速率，並執行轉換矩陣的 計算，該轉換矩陣計算模塊將該轉換矩陣送給該坐標轉換 計算模塊和該姿態位置速度計算模塊，該轉換矩陣計算模 塊中的一姿態更新算法利用四元素法進行姿態計算，其中 該坐標轉換模塊從該I MU誤差計算模塊接收該比力，從該 轉動矩陣計算模塊接收轉動矩陣，並執行該坐標轉換，該 坐標轉換計算模塊將轉換到由該轉換矩陣表徵的坐標系中 的比力送給該姿態位置速度計算模塊，其中該姿態位置速 度計算模塊接收從該坐標轉換計算模塊接收該轉換後的比 力，從該轉換矩陣計算模塊接收該轉換矩陣，並更新該姿 態，位置，和速度者。 5. 如專利申請範圍第4項所述之一增強型導航定位系統， 其中，在進行該位置和速度計算之後，由卡爾曼濾波器計 算得來的該位置和速度誤差用在該姿態位置速度計算模塊 中來校正該慣性解者。 6. 如專利申請範圍第5項所述之一增強型導航定位系統， 其中，由該卡爾曼濾波器計算得出的姿態誤差送給該姿態 位置速度計算模塊，並在該姿態位置速度計算模塊中執行 姿態校正者。 7. 如專利申請範圍第5項所述之一增強型導航定位系統，

   第55頁 531657 六、申請專利範圍 其中由該卡爾曼濾波器計算得出的姿態誤差送給該轉換矩 陣計算模塊，並在姿態位置速度計算模塊之前執行姿態校 正者。 8. 如專利申請範圍第6項所述之一增強型導航定位系統， 其中，由該姿態位置速度計算模塊得來的校正後的慣性解 送給該卡爾曼濾波器來建立該卡爾曼濾波器的觀測量，同 時校正後的慣性解送給該載波相位整周模糊求解模塊來輔 助求解全球定位系統衛星信號載波相位整周模糊值，校正 後的速度和加速度信息送給GPS處理器的該微處理器來輔 助全球定位系統衛星信號的載波相位和碼跟蹤，其中由姿 態位置速度計算模塊計算得來的速度送給該地球和運載體 轉動速率計算模塊來計算地球轉動速率和運栽體轉動速 率，該地球轉動速率和運載體轉動速率送給該轉換矩陣計 算模塊，其中該姿態，位置和速度信息送給該I / 0接口 者。 9. 如專利申請範圍第7項所述之一增強型導航定位系統， 其中，由該姿態位置速度計算模塊得來的校正後的慣性解 送給該卡爾曼濾波器來建立該卡爾曼濾波器的觀測量，同 時校正後的慣性解送給該載波相位整周模糊求解模塊來輔 助求解全球定位系統衛星信號載波相位整周模糊值，校正 後的速度和加速度信息送給GPS處理器的該微處理器來輔 助全球定位系統衛星信號的載波相位和碼跟蹤，其中由姿 態位置速度計算模塊計算得來的速度送給該地球和運載體 轉動速率計算模塊來計算地球轉動速率和運載體轉動速

   第56頁 531657 六、申請專利範圍 f ’該地球轉動速率和運栽體轉動速率送給該轉換矩陣計 鼻模塊，其中該姿態，位置和速度信息送給該丨/〇接口 者0 1 〇 ·如專利申請範圍第8項、或第9項所述之一增強型導航 定位系統，其中所述卡爾曼濾波器是一強健型卡 器，能夠為一系列混淆所述全球定位系統量測值，所^ 度測量裝置，以及所述慣性傳感器測量得過程和模型提供 進於優化的濾杷，該穩健卡爾曼濾波器包括一Gps誤差/ 核杈塊用於從該GPS處理器接收該Gps觀測量，包談 ξ晉f m 11卜勒頻率，從一狀態向量更新模塊接收 位置和速度校正I，並執行GPS誤差補償以生成校正過的 GPS原始數據’ ϋ將該GPS原始數據送給一預處理模塊，該 預處理杈塊從該GPS處理器接收Gps衛星星歷數據，從該 GPS誤差補償模塊接收該校正後的Gps原始數據，從該^ 測ϊ裝置接收該運載體高度觀測量，從該丨NS處理器 INS導航解，該預處理模塊計算狀態傳輸矩陣，並將該^ 態傳輸矩陣連同該狀態向量送給一狀態向量預測模塊， 計异得到的狀態傳輸矩陣也送給一協方差傳播模塊，該^ 方差傳播模塊根據一計算得到的觀測矩陣和一觀測模^ 算一觀測矩陣和當前觀測向量，該觀測矩陣和該當前觀 向量送給一觀測量殘差計算模塊，該狀態向量預測模塊從 該預處理模塊接收該狀態傳輸矩陣和該狀態向量，並對卷 前^刻的狀，進行預測，該預測的當前狀態向量送給該& 測量殘差计算模塊’該觀測量殘差計算模塊從該狀態向量

   第57頁 531657 六、申請專利範圍 預測模塊接收預測的當前狀態向量，從該預處理模塊接收 該測量矩陣和該當前觀測向量，該觀測量殘差計算模塊通 過從该當鈿觀測向量減去該觀測矩陣與該當前狀態向量預 ’則值的乘積來计算觀測量殘差，該殘差監视模塊對從該觀 剛量殘差計算模塊來的觀測量殘差進行判別，該協方差傳 ^巧塊從該殘差監視模塊接收系統過程的協方差，從預處 姑餐塊接收狀態傳輪矩陣和估計誤差的協方差來計算當前 計ί誤差的協方差，該估計誤差的協方差送給一最優增益 场算模塊，該最優增益計算模塊從該協方差計算模塊^收 g t計誤差的當前協方差，並計算最優增益，該最優增益 更给二協方差更新模塊和該狀態向量更新模塊，該協^ ^ 方f模塊更新該估計誤差的協方差，並將該估計誤差的協 優ΐ送、t ΐ，方差傳播矩陣，該狀態向量更新模塊從該最 绳=接收該最優增益，從該觀測量殘差計算模 向ί 殘I，該狀態向量更新模塊計算當前狀態 度^ =估什值，包括位置，速度和姿態誤差，該位置，速 u態誤差送給該GPS誤差補償模塊和該INS處理器 定知\專利申請範圍第8項、或第9項所述之一增強型導航 位ίΐ該載波t位整周模糊求解模塊從該1^處 • 速度數據’從該GPS處理器的該微處理器 I =協方差矩陣來確定該全球定位系統衛星信號載;=: 楹2楔糊值，並將該全球定位系統衛星信號載波相1 、蝴值送給該卡爾曼濾波器以進一步提高該GPS原始觀則*

   第58頁

   531657 六、申請專利範圍 量的觀測精度者。 1 2 .如專利申請範圍第1 1項所述之一增強型導航定位系 統，其中該載波相位整周模糊求解模塊包括一幾何距離計 算模塊，一最小平方調整模塊，一衛星鐘模型，一電離層 模型，一對流層模型，一衛星預測模塊，和一收索空間確 定模塊，該衛星預測模塊從該GPS處理器接收可見GPS衛星 的星歷數據，並進行衛星位置的計算，該衛星位置送給該 幾何距離計算模塊，該幾何距離計算模塊從該I N S處理器 接收運載體精確的位置信息，並計算由運載體到衛星之間 的幾何距離，該幾何距離送給該最小平方調整模塊，該對 流層模型從該GPS處理器接收一時間標誌，並用一嵌入的 對流層延遲模型計算該GPS衛星信號的對流層延遲，該對 流層延遲送給該最小平方調整模塊，該電離層模型從該 GPS處理器接收該GPS衛星信號的時間標誌和電離層參數， 並計算電離層延遲，該電離層延遲送給該最小平方調，整模 塊，該衛星鐘模型接收GPS衛星鐘參數，並進行衛星鐘校 正的計算，該衛星鐘校正量也送給該最小平方調整模塊， 該收索空間確定模塊接收觀測量誤差並確定全球定位系統 衛星信號載波相位整周模糊值收索空間，該收索空間也送 給該最小平方調整模塊，該最小平方調整模塊從該幾何距 離計算模塊接收從運載體到衛星之間的幾何距離，從該對 流層模型接收該對流層延遲，從該電離層模型接收該電離 層延遲，從衛星鐘模型接收該衛星鐘校正量，並計算一初 始的收索原點，該最小平方調整模塊從該收索空間確定模

   第59頁 531657 六、申請專利範圍 塊接收該整周模糊值收索空間，並用一標準的最小平方調. 整算法求解該載波相位整周模糊值者。 1 3 . —種增強型導航定位系統包括： 一全球定位系統GPS處理器用於提供與位置有關的信 息； 一慣性測量單元I MU用於提供慣性觀測量，包括載體 的轉動角速率和比力； 一高度觀測量產生器用於提供載體高度觀測量，其中 所述高度觀測量產生器是一個雷達高度計，用以提供運動 器地面以上的高度； 一中心導航處理器，它與該GPS處理器，該IMU和該高 度觀測量產生器相連，並用於融合來自該GPS處理器的該 與位置有關的信息，來自該I M U的該慣性觀測量和來自該 高度觀測量產生器的該高度觀測量來產生載體的導航解， 包括位置，速度，和姿態； 一輸入/輸出（I/O)接口 ，它與該中心導航處理器相 連，用於輸出該導航解； 其中所述全球定位系統處理器產生一組偽距，載杷相位和 多普勒頻移；其中所述中央航行處理器包括一個慣性導航 系統處理器，一個卡爾曼濾波器，一個載杷相位模糊整數 求解單元；其中所述偽距，載杷相位和多普勒頻移被傳送 到中央航行處理器，所述地面以上高度量測值被傳送到卡 爾曼濾波器，所述慣性量測值被傳送到慣性導航系統處理 器；其中所述慣性導航系統處理器的輸出值，所述平均海

   第60頁 531657 六、申請專利範圍 平面以上高度量測值，所述偽距，載杷相位和多普勒頻移 在卡爾曼滤波器中被混合，而所述卡爾曼遽波器的輸出則 被反饋到所述慣性導航系統處理器中用來校正從所述中央 航行處理器輸出到所述I / 〇界面的一個慣性導航系統結 果；其中所述慣性導航系統處理器為所述全球定位系統處 理器提供速度和加速度數據以跟蹤全球定位系統衛星信 號；全球定位系統處理器，慣性導航系統處理器，以及卡 爾曼濾波器的輸出被發送到所述載杷相位模糊整數求解模 塊以求得全球定位系統載杷相位模糊整數；所述載杷相位 模糊整數求解模塊將所求得的載杷相位反饋到卡爾曼濾波 器以進一步提供定位精度；其中所述慣性導航系統處理器 將導航數據輸出到所述I / 0界面； 其中所述全球定位系統處理器的微處理器將所述偽 距，載杷相位以及多普勒頻移，全球定位系統衛星星歷， 以及大氣參數發送給所述卡爾曼濾波器； 其中所述慣性導航系統處理器處理所述慣性量測值， 包括體角速度和外力，以及由所述卡爾曼濾波器輸出的方 位誤差，速度誤差以及高度誤差以校正導航結果； 其中該雷達高度計將該地形高度測量值送給該數據融 合模塊，該I N S處理器將運載體的位置信息送給該地形數 據庫，該地形數據庫執行數據庫查詢，並導出地面相對於 海平面的高度，並將該地面相對於海平面的高度送給該數 據融合模塊，該數據融合模塊從該雷達高度計接收該地形 高度測量值和該地面相對於海平面的高度，並導出運載體

   第61頁 531657 六、申請專利範圍 相對於平均海平面的高度，該相對於平均海平面的高度送 給該卡爾曼濾波器者。 1 4 ·如專利申請範圍第1 3項所述之一增強型導航定位系 統’其中該INS處理器包括一 IMUI/0接口 ，一imu誤差補償 模塊’ 一坐標轉換計算模塊，一姿態位置速度計算模塊， 一轉換矩陣汁异模塊，和一地球和運動體轉動速率計算模 塊，其中該IMU I/O接口從該丨㈣接收該機體轉動角速率和 比力#號，並處理轉化為數字數據，這些數字數據迭加有 该慣性傳感器測ϊ疾差’該受慣性傳感器測量誤差污染的 慣性測ϊ值送給该I M U誤差補償模塊，其中該I μ u誤差^償 模塊接收來自該卡爾曼滤波器的傳感器誤差二um 行I MU誤差校正，該校正後的慣性數據送給該坐棹轉換 算模塊和該轉換矩陣計算模塊，該比力送給該坐標轉換 算模塊，該轉換矩陣計算模塊從該[MU誤差計算模塊接收 該機體角速率，從該地球和運載體轉動速率計算模塊接收 一地球和運，體轉動角速率，並執行轉換矩陣的計算該 轉換矩陣計算模塊將該轉換矩陣送給該坐標 和該姿”置速度計算模塊，該轉換矩陣計：模 ί ί ^新具法利用四70素法進行姿態計算，其中該坐標轉 才^矣塊從該IMU誤差計算模塊接收該比力，從該轉動矩陣# ^ ί ί塊接收轉動矩陣，並執行該坐標轉換’該坐標轉換 =#柄产將轉換到由該轉換矩陣表徵的坐標系中的比力送 :ί姿態位置速度計算模塊，其中該姿態位置速度計算模 塊接收從該坐標轉換計算模塊接收該轉換後的比力，從該

   531657 六、申請專利範圍 轉換矩陣計算模塊接收該轉換矩陣，並更新該姿態，位 置，和速度者。 1 5.如專利申請範圍第1 4項所述之一增強型導航定位系 統，其中在進行該位置和速度計算之後，由卡爾曼遽波器 計算得來的該位置和速度誤差用在該姿態位置速度計算模 塊中來校正該慣性解者。 1 6 .如專利申請範圍第1 5項所述之一增強型導航定位系 統，其中由該卡爾曼濾波器計算得出的姿態誤差送給該姿 態位置速度計算模塊，並在該姿態位置速度計算模塊中執 行姿態校正者。 1 7.如專利申請範圍第1 5項所述之一增強型導航定位系 統，其中由該卡爾曼濾波器計算得出的姿態誤差送給該轉 換矩陣計算模塊，並在姿態位置速度計算模塊之前執行姿 態校正者。 1 8 .如專利申請範圍第1 6項所述之一增強型導航定位系 統，其中由該姿態位置速度計算模塊得來的校正後的慣性 解送給該卡爾曼濾波器來建立該卡爾曼濾波器的觀測量， 同時校正後的慣性解送給該載波相位整周模糊求解模塊來 輔助求解全球定位系統衛星信號載波相位整周模糊值，校 正後的速度和加速度信息送給G P S處理器的該微處理器來 輔助全球定位系統衛星信號的載波相位和碼跟蹤，其中由 姿態位置速度計算模塊計算得來的速度送給該地球和運載 體轉動速率計算模塊來計算地球轉動速率和運載體轉動速 率，該地球轉動速率和運載體轉動速率送給該轉換矩陣計

   第63頁 531657 t、申請專利範圍 算模塊，其中該姿態，位置和速度信息送給該i / 〇接口 者。 1 9 .如專利申請範圍第1 7項所述之一增強型導航定位系 統，其中由該姿態位置速度速度計算模塊得來的校正後的 慣性解送給該卡爾曼濾波器來建立該卡爾曼濾波器觀測 量，同時校正後的慣性解送給該載波相位整周模糊求解模 塊來辅助求解全球定位系統衛星信號載波相位整周模糊 值，校正後的速度和加速度信息送給GPS處理器的該微處 理器來輔助全球定位系統衛星信號的載波相位和碼跟蹤， 其中由姿態位置速度計算模塊計算得來的速度送給該地球 和運載體轉動速率計算模塊來計算地球轉動速率和運載體 轉動速率，該地球轉動速率和運載體轉動速率送給該轉換 矩陣計算模塊，其中該姿態，位置和速度信息送給該I / 〇 接口者。 2 0 .如專利申請範圍第1 8項、或第1 9項所述之一增強型導 航定位系統，其中所述卡爾曼濾波器是一強健型卡爾曼濾 波器，能夠為一系列混淆所述全球定位系統量測值，所述 高度測量裝置，以及所述慣性傳感器測量得過程和模型提 供進於優化的濾杷；該穩健卡爾曼濾波器包括一GPS誤差 補償模塊用於從該GPS處理器接收該GPS觀測量，包括該偽 距，載波相位和多卜勒頻率，從一狀態向量更新模塊接收 位置和速度校正量，並執行GPS誤差補償以生成校正過的 GPS原始數據，並將該GPS原始數據送給一預處理模塊，該 預處理模塊從該GPS處理器接收GPS衛星星歷數據，從該

   第64頁 531657

   塊接收該校正後的GPS原始數據，從該數據 二δ/Λ 運載體高度觀測量，從該ins處理器接收 能僂於拓睡，/預處理模塊計算狀態傳輸矩陣，並將該狀 ;:”陣連同該狀態向量送給一狀態向量預測模塊，該 狀態傳輸矩陣也送給一協方差傳播模塊，該協 算-】ΐΐΐ根據一計算得到的觀測矩陣和-觀測模型計 Γ和當前觀測向*，該觀測矩陣和該當前觀測 量殘差計算模塊，該狀態向量預測模塊從 預測，該預測的當前狀態向量送給：： 預例槿μ ^ ΐ'，忒觀測量殘差計算模塊從該狀態向量 ίΠΐ收預；的當前狀態向，，從該預處理模塊接收 :從3:=二：觀測向*，該觀測量殘差計算模塊通 測= 2 ΐ該觀測矩陣與該當前狀態向量預 測量殘差叶算^堍炎2夏殘差該殘差監視模塊對從該觀 异塊來的觀測量殘差進行判岁】，該協方差傳 理模塊接收狀態傳輪矩= ，從預處 送給-協方差更新模塊和該iC…最優增益 更新模塊更新該估計誤差的協^!更新模塊，該協方差 方差送給該協方差傳播矩：協：；：並將ΐ估計誤差的協 φ矩陣該狀態向量更新模塊從該最

   第65頁 531657 六、申請專利範圍 優增益計算模塊接收該最優增益，從該觀測量殘差計算模 塊接收該觀測量殘差，該狀態向量更新模塊計算當前狀態 向量的估計值，包括位置，速度和姿態誤差，該位置，速 度和姿態誤差送給該GPS誤差補償模塊和該INS處理器者。

   2 1 ·如專利申請範圍第1 8項、或第1 9項所述之一增強型導 航定位系統’其中該載波相位整周模糊求解模塊從該I NS 處理器接收位置和速度數據，從該GPS處理器的該微處理 器接收該載波相位和多卜勒頻率觀測量，從該卡爾曼濾波 器接收協方差矩陣來確定該全球定位系統衛星信號載波相 位整周模糊值，並將該全球定位系統衛星信號載波相位整 周模糊值送給該卡爾曼濾波器以進一步提高該Gps原始觀 測量的觀測精度者。

   22·如專利申請範圍第2 1項所述之一增強型導航定位系 統’其中該載波相位整周模糊求解模塊包括一幾何距離計 算模塊，一最小平方調整模塊，一衛星鐘模型，一電離層 模型，一對流層模型，一衛星預測模塊，和一收索空間確 疋模塊’該衛星預測模塊從該GPS處理器接收可見⑶^衛星 的星歷數據’並進行衛星位置的計算，該衛星位置送給該 幾何距離計算模塊，該幾何距離計算模塊從該丨NS處理器 接收運載體精確的位置信息，並計算由運載體到衛星之間 的幾何距離，該幾何距離送給該最小平方調整模塊，該對 流層模型從該GPS處理器接收一時間標誌，並用一嵌入的 對流層延遲模型計算該GPS衛星信號的對流層延遲，該對 流層延遲送給該最小平方調整模塊，該電離層模型從該

   第66頁 531657 六、申請專利範圍 GPS處理器接收該GPS衛星信號的時間標誌和電離層參數， 並計算電離層延遲，該電離層延遲送給該最小平方調整模 塊，該衛星鐘模型接收GPS衛星鐘參數，並進行衛基鐘校 正的計算，該衛星鐘校正量也送給該最小平方調整模塊， 該收索空間確定模塊接收觀測量誤差並確定全球定位系統 衛星信號載波相位整周模糊值收索空間，該收索空間也送 給該最小平方調整模塊，該最小平方調整模塊從該幾何膝 離計算模塊接收從運載體到衛星之間的幾何距離，從該對 流層模型接收該對流層延遲，從該電離層模型接收該電離 層延遲，從衛星鐘模型接收該衛星鐘校正量，並計算〆初 始的收索原點，該最小平方調整模塊從該收索空間確定模 塊接收該整周模糊值收索空間，並用一標準的最小平方調 整算法求解該載波相位整周模糊值者。 2 3. —增強型導航定位系統包括： 一全球定位系統GPS處理器用於提供與位置有關的信 息； 一慣性測量單元ΙΜϋ用於提供慣性觀測量，包括載體 的轉動角速率和比力； 一南度觀測量產生器用於提供載體高度觀測量；其中 所述高度測量產生器是一個高度測量裝置用來產生平均海 平&以上得高度測量值； —中心導航處理器，它與該GPS處理器，該IMU和該高 度觀測量產生器相連，並用於融合來自該GPS處理器的該 與位置有關的信息，來自該IMU的該慣性觀測量和^自該

   531657 六、申請專利範圍 高度觀測量產生器的該高度觀測量來產生載體的導航解， 包括位置，速度，和姿態； —輪入/輸出（I/O)接口，它與該中心導航處理器相 連’用於輸出該導航解； 其中所述全球定位系統處理器產生一組偽距，栽杷相 位和多普勒頻移；其中所述中央航行處理器包括一個慣性 導航系統處理器，一個卡爾曼濾波器，一個載杷相位模糊 整數求解單元；其中所述偽距，載杷相位和多普勒頻移被 傳送到中央航行處理器，所述平均海平面以上高度量測值 被傳送到卡爾曼濾波器，所述慣性量測值被傳送到慣性導 航系統處理器；其中所述慣性導航系統處理器的輸出值， 所述平均海平面以上高度量測值，所述偽距，載杷相位和 多普勒頻移在卡爾曼濾波器中被混合，而所述卡爾曼遽波 器的輸出則被反饋到所述慣性導航系統處理器中用來校正 從所述中央航行處理器輸出到所述I /0界面的一個慣性導 航系統結果； 所述全球定位處理器得微處理器輸出所述偽距，載把 相位，多普勒頻移，全球定位系統星歷，以及大氣象數终 所述卡爾曼濾波器； / ^ 所述慣性導航系統處理器處理所述慣性量測值，包括 體角速度力，以及從卡爾曼濾波器得到的/方位誤1’ 速度誤差’高度誤差以推導經過校正的導航結果. 所述：度測量裝置將所述平均海平面以：的高度測量 值發送給卡爾曼濾波器者。

   531657 、、申請專利範圍

   24·如專利申請範圍第23項所述之一增強型導航定位系 先其中该IN S處理器包括一 IM UI / 0接口，一 IΜ11誤差補償 模塊’ 一坐標轉換計算模塊，一姿態位置速度計算模塊， 轉換矩陣計算模塊，和一地球和運動體轉動速率計算模 塊’其中該IMU I/O接口從該IMU接收該機體轉動角速率和 =力信號，並處理轉化為數字數據，這些數字數據迭加加 有該慣性傳感器測量誤差，該受慣性傳感器測量誤差污染 的慣性測量值送給該IMlJ誤差補償模塊，其中者 償模塊接收來自該卡爾曼濾波器的傳感器誤差估計 H 執^ I MU誤差校正，該校正後的慣性數據送給該坐標轉換 計^模塊和該轉換矩陣計算模塊，該比力送給該坐標轉換 计算模塊，該轉換矩陣計算模塊從該丨Μϋ誤差計算模塊接 收該機體角速率，從該地球和運載體轉動速率計算模塊接 收一地球和運載體轉動角速率，並執行轉換矩陣的計算， 忒轉換矩陣计算模塊將該轉換矩陣送給該 塊和該姿態位置速度計算模塊，該轉換矩陣 一姿態更新算法利用四元素法進行姿態計算，其中該坐標 轉換模塊從該I MU誤差計算模塊接收該比力，從該轉動矩 陣計算模塊接收轉動矩陣，並執行該坐標轉換，該坐標轉 換計算模塊將轉換到由該轉換矩陣表徵的坐標系中的比力 送給該錢位置速度計算模土鬼，&中料態位置速度計算 模塊接收從該坐標轉換計算模塊接收該轉換後的比力從 該轉換矩陣計异模塊接收該轉換矩陣，並更新該姿態，位 置，和速度者。

   531657 六、申請專利範圍 p从各 25.如專利申請範圍第24項所述之一增強型導航疋、 統，其中在進行該位置和速度計算之後，由卡爾；J'ff 計算得來的該位置和速度誤差用在該姿態位置速度什异模 塊中來校正該慣性解者。 / 2 6 ·如專利申請範圍第2 5項所述之一增強型導航疋位系 統，其中由該卡爾曼滤波器計算得出的姿態誤羞送給該姿 態位置速度計算模塊，並在該姿態位置速度計算模塊中執 行姿態校正者。 2 7·如專利申請範圍第26項所述之一增強型導航定位系 統，其中由該卡爾曼濾波器計算得出的姿態誤差送給該轉 f矩陣計算模塊，並在姿態位置速度計算模塊之前執行姿 態校正者。 28·如專利申請範圍第25項、 $強型導航定位系統，其中 ^的权正後的慣性解送給該 %、波器的觀測量，同時校正 f模糊求解模塊來輔助求解 整周模糊值，校正後的速 為的該微處理器來辅助全球 跟蹤，其中由姿態位置 該地球和運栽體轉動速 丰和運載體轉動速率， ^二遠轉換矩陣計算模塊’ 、給該I/O接口者。 第26項’ 由該姿態 卡爾曼滤 後的慣性 全球定位 度和加速 定位系統 速度計算 率計算模 球轉動速 其中該姿 或第2 7項所述之 位置速 波器來 解送給 系統衛 度信息 衛星信 模塊計 塊來計 率和運 態，位 度計算模塊得 建立該卡爾曼 該載波相位整 星信號載波相 送給GPS處理 號的載波相位 算得來的速度 算地球轉動速 載體轉動速率 置和速度信息

   第70頁 531657 六、申請專利範圍 29·如專利申請範圍第28項所述之一增強型導航定位系 統，其中所述卡爾曼濾波器是一強健型卡爾曼濾波器，能 夠為一系列混淆所述全球定位系統量測值，所述高度測量 裝置，以及所述慣性傳感器測量得過程和模型提供進於優 化的濾杷；該穩健卡爾曼濾波器包括一GPS誤差補償模塊 用於從該GPS處理器接收該GPS觀測量，包括該偽距，載波 相位和多卜勒頻率，從一狀態向量更新模塊接收位置和速 度校正量，並執行GPS誤差補償以生成校正過的GPS原始數 據，並將該GPS原始數據送給一預處理模塊，該預處理模 塊從該GPS處理器接收GPS衛星星歷數據，從該GPS誤差補 償模塊接收該校正後的GPS原始數據，從該高度測量裝置 接收該運載體高度觀測量，從該INS處理器接收INS導航 解，該預處理模塊計算狀態傳輸矩陣，並將該狀態傳輸矩 陣連同該狀態向量送給一狀態向量預測模塊，該計算得到 的狀態傳輸矩陣也送給一協方差傳播模塊，該協方差傳播 模塊根據一計算得到的觀測矩陣和一觀測模型計算一觀測 矩陣和當前觀測向量，該觀測矩陣和該當前觀測向量送給 一觀測量殘差計算模塊，該狀態向量預測模塊從該預處理 模塊接收該狀態傳輸矩陣和該狀態向量，並對當前時刻的 狀態進行預測’該預測的當前狀態向量送給該觀測量殘差 計算模塊，该觀測量殘差計算模塊從該狀態向量預測模塊 接收預測的當前狀態向量，從該預處理模塊接收該測量矩 陣和該當前觀測向量，該觀測量殘差計算模塊通過從該當 前觀測向量減去該觀測矩陣與該當前狀態向量預測值的^

   531657 ^、申請專利範圍 " --- 積來计算觀測$殘差’該殘差監視模塊對從該觀測量殘差 計算模塊來的觀測量殘差進行判別，該協方差傳播模塊從 該殘差監視模塊接收系統過程的協方差，從預處理模塊接 收狀態傳輸矩陣和估計誤差的協方差來計算當前估計誤差 的協方差，戒估計誤差的協方差送給一最優增益計算模 塊’該最優增益計算模塊從該協方差計算模塊接收該估計 誤差的當前協方差’並計算最優增益，該最優增益送給一 協方差更新模塊和該狀態向量更新模塊，該協方差更新模 塊更新該估計誤差的協方差，並將該估計誤差的協方差送 給該協方差傳播矩陣，該狀態向量更新模塊從該最優增益 計算模塊接收該最優增益，從該觀測量殘差計算模塊接收 該觀測量殘差，該狀態向量更新模塊計算當前狀態向量的 估計值，包括位置，速度和姿態誤差，該位置，速度和姿 態誤差送給該GPS誤差補償模塊和該ins處理器者。 3 0. 一種增強型導航定位系統包括： 一全球定位系統GPS處理器用於提供與位置有關的信 息〃 一慣性測量單元IM U用於提供慣性觀測量，包括載體 的轉動角速率和比力； 一高度觀測量產生器用於提供載體高度觀測量；其中 所述高度測量裝置是一個雷達高度計用以提供運動體到地 面的高度； 一中心導航處理器，它與該GPS處理器，該1MU和該高 度觀測量產生器相連，並用於融合來自該GPS處理器的該

   531657 六、申請專利範圍 與位置有關的信息，來自該IMU的該慣性觀測量和來自該 *度觀測量產生器的該高度 包括位置，速度，和姿態； 一輪入/輸出（I/O)接口 連’用於輪出該導航解； 其中所述全球定位系統 位和多普勒頻移；其中所述 導航系統處理器，一個卡爾 整數求解單元；其中所述偽 傳送到中央航行處理器，所 被傳送到卡爾曼濾波器，所 航系統處理器；其中所述慣 所述平均海平面以上高度量 多普勒頻移在卡爾曼濾波器 器的輸出則被反饋到所述慣 從所述中央航行處理器輸出 航系統結果； 所述全球定位處理器得 相位，多普勒頻移，全球定 所述卡爾曼濾波器； 所述慣性導航系統處理 體角速度，外力，以及從卡 速度誤盖’高度誤差以推導 所述高度測量裝置將所 觀測量來產生載體的導航解， ，它與該中心導航處理器相 處理器產生一組偽距，載杷相 中央航行處理器包括一個慣性 曼濾波器，一個載杷相位模糊 距’載杷相位和多普勒頻移被 述平均海平面以上高度量測值 述慣性量測值被傳送到慣性導 性導航系統處理器的輸出值， 測值，所述偽距，載杷相位和 中被混合，而所述卡爾曼濾波 性導航系統處理器中用來校正 到所述I / 0界面的一個慣性導 微處理器輸出所述偽距，載杷 位系統星歷，以及大氣參數給 器處理所述慣性量測值，包括 爾曼綠、波器得到的方位誤差， 經過校正的導航結果；、 述平均海平面以上的高度測量

   第73頁 531657 六、申請專利範圍 值發送給卡爾曼濾波器者。 3 1 ·如專利申請範圍第3 0項所述之一增強型導航定位系 統’其中該INS處理器包括一 IMU I/O接口 ’ 一IMU誤差補償 模塊’ 一坐標轉換計算模塊，一姿態位置速度計算模塊， 轉換矩陣計算模塊，和一地球和運動體轉動速率計算模 塊’其中該IMU I/O接口從該IMU接收該機體轉動角速率和 比力信號，並處理轉化為數字數據，這些數字數據迭加加 有該慣性傳感器測量誤差，該受慣性傳感器測量誤差污染 ^慣性測量值送給該IMU誤差補償模塊，其中該imu誤差補 償模塊接收來自該卡爾曼濾波器的傳感器誤差估計值，並 執行IMU誤差校正，該校正後的慣性數據送給該坐標轉換 計算模塊和該轉換矩陣計算模塊，該比力送給該坐標轉換 計算模塊，該轉換矩陣計算模塊從該丨MlJ誤差計算模塊接 收該機體角速率，從該地球和運載體轉動速率計算模塊接 收一地球和運載體轉動角速率，並執行轉換矩陣的計算， 該轉換矩陣計算模塊將該轉換矩陣送給該坐標轉換計算模 塊和該姿態位置速度計算模塊，該轉換矩陣計算模塊中的 一姿態更新算法利用四元素法進行姿態計算，其中該坐標 轉換，塊從該IMU誤差計算模塊接收該比力，從該轉動矩 陣計$模塊接收轉動矩陣，並執行該坐標轉換，該坐標轉 換計算模塊將轉換到由該轉換矩陣表徵的坐標系中的比力 送給戎姿態位置速度計算模塊，其中該姿態位置速度計算 模塊接收從該坐標轉換計算模塊接收該轉換後的比力，從 該轉換矩陣計算模塊接收該轉換矩陣，並更新該姿態，位

   第74頁 531657

   六、申請專利範圍 置，和速度者。 3 2 ·如專利申請範圍第3 1項所述之一增強型導航定位系q 統，其中在進行該位置和速度計算之後，由卡爾曼遽波器 計算得來的該位置和速度誤盖用在該姿態位置速度計算模 塊中來校正該慣性解者。 3 3·如專利申請範圍第32項所述之一增強型導航定位系 統，其中由該卡爾曼濾波器計算得出的姿態誤差送給該姿 態位置速度計算模塊，並在該姿態位置速度計算模塊中執 行姿態校正者。 34·如專利申請範圍第32項所述之一增強型導航定位系 統，其中由該卡爾曼濾波器計算得出的姿態誤差送給該轉 換矩陣計算模塊，並在姿態位置速度計算模塊之前執行姿 態校正者。 3 5·如專利申請範圍第32項、第33項、或第34項所述之一 增強型導航定位系統，其中由該姿態位置速度計算模塊得 來的校正後的慣性解送給該卡爾曼濾波器來建立該卡爾曼 濾、波器的觀測量，同時校正後的慣性解送給該載波相位整 周模糊求解模塊來輔助求解全球定位系統衛星信號載波相 位整周模糊值’校正後的速度和加速度信息送給Gps處理 器的该微處理器來輔助全球定位系統衛星信號的載波相位 和碼跟蹤，其中由姿態位置速度計算模塊計算得來的速度 送給該地球和運載體轉動速率計算模塊來計算地球轉動速 率和運載體轉動速率，該地球轉動速率和運載體轉動速率 送給該轉換矩陣計算模塊，其中該姿態，位置和速度信息

   第75頁 531657 六、申請專利範圍 送給該I/O接口者。 3 6 ·如專利申請範圍第3 5項所述之一增強型導航定位系 統，其中所述卡爾曼濾波器是一強健型卡爾曼濾波器，能 夠為一系列混淆所述全球定位系統量測值和所述慣性傳感 器測量得過程和模型提供進於優化的濾波者。 3 7 ·如專利申請範圍第2 8項所述之一增強型導航定位系 統’其中該穩健卡爾曼濾波器包括一GPS誤差補償模塊用 於從該GPS處理器接收該GPS觀測量，包括該偽距，載波相 位和多卜勒頻率’從一狀態向量更新模塊接收位置和速度 校正量，並執行GPS誤差補償以生成校正過的GPS原始數 據’並將該G P S原始數據送給一預處理模塊，該預處理模 塊從該GPS處理器接收GPS衛星星歷數據，從該GPS誤差補 償模塊接收該校正後的GPS原始數據，從該高度測量裝置 接收該運載體高度觀測量，從該I M S處理器接收IN S導航 解’該預處理模塊計算狀態傳輸矩陣，並將該狀態傳輸矩 陣連同該狀態向量送給一狀態向量預測模塊，該計算得到 的狀態傳輸矩陣也送給一協方差傳播模塊，該協方差傳播 模塊根據一計算得到的觀測矩陣和一觀測模型計算一觀測 矩陣和當前觀測向量，該觀測矩陣和該當前觀測向量送給 一觀測里殘差叶异模塊，該狀態向量預測模塊從該預處理 模塊接收該狀態傳輪矩陣和該狀態向量，並對當前時刻的 狀態進行預測，該預測的當前狀態向量送給該觀測量殘差 計算模塊’該觀測量殘差計算模塊從該狀態向量預測模塊 接收預測的當前狀態向量，從該預處理模塊接收該測量矩

   第76頁 531657 六、申請專利範圍 ”’該觀測量殘差計算模塊通過從該當 則觀測向置減去该觀測矩陣與該當前狀態 積來計算觀測量殘差，該殘差監視模塊姐里預刺值的 你叹友孤仇供％對從該 晋碚差 計算模塊來的觀測量殘差進行判別，哕士 *觀則里殘 、 X忘xe 列乃j成協方差傳播模塊從 該殘差監視模塊接收系統過程的協方差，從預處理模塊接 收狀態傳輸矩陣和估計誤差的協方差來計算當前估計誤差 的協方差’該估計誤差的協方差送給一最優^益計算模 塊，該最優增益計算模塊從該協方差計算模塊接收該估計 誤差的當如協方差’並計算最優增益，該最優增益送給一 協方差更新模塊和該狀態向量更新模塊，該協方差更新模 塊更新該估計誤差的協方差，並將該估計誤差的協方差送 給該協方差傳播矩陣，該狀態向量更新模塊從該最優增益 計算模塊接收該最優增益，從該觀測量殘差計算模塊接收 該觀測量殘差，該狀態向量更新模塊計算當前狀態向量的 估計值，包括位置，速度和姿態誤差，該位置，速度和姿 態誤差送給該GPS誤差補償模塊和該INS處理器者。 38· —種增強型導航定位系統包括： 一全球定位系統GPS處理器用於提供與位置有關的信 息； 一慣性測量單元IMU用於提供慣性觀測量’包括載體 的轉動角速率和比力； 一高度觀測量產生器用於提供載體高度觀測量；其中 所述高度測量裝置是一個高度測量裝置用以提供運動體到 平均海平面的高度；

   第77頁 531657 六、申請專利範圍 一中心導航處理器，它與該GPS處理器，該IMU和該高 度觀測量產生器相連，並用於融合來自該G P S處理器的該 與位置有關的信息，來自該IMU的該慣性觀測量和來自該 高度觀測量產生器的該高度觀測量來產生載體的導航解， 包括位置’速度，和姿態； 一輸入/輸出（I/O)接口，它與該中心導航處理器相 連，用於輸出該導航解； 其中所述全球定位糸統處理產生一組偽距，載把相位和 多普勒頻移；其中所述中央航行處理器包括一個慣性導航 糸統處理器’一個卡爾曼濾波器，一個載把相位模糊整數 求解單元；其中所述偽距，載杷相位和多普勒頻移被傳送 到中央航行處理器，所述平均海平面以上高度量測值被傳 送到卡爾曼濾波器，所述慣性量測值被傳送到慣性導航系 統處理器；其中所述慣性導航系統處理器的輸出值，所述 平均海平面以上高度量測值，所述偽距，載杷相位和多普 勒頻移在卡爾曼遽波器中被混合，而所述卡爾曼濾波器的 輸出則被反饋到所述慣性導航系統處理器中用來校正從所 述中央航行處理器輸出到所述I / 〇界面的一個慣性導航系 統結果； 所述全球定位處理器得微處理器輪出所述偽距，栽把 相位，多普勒頻移，全球定位系統星歷，以及大氣參數給 所述卡爾曼濾波器； 所述慣性導航系統處理器處理所述慣性量測值，包括 體角速度，外力，以及從卡爾曼濾波器得到的方位誤差，

   第78頁 531657

   速度誤差，高度誤差以推導經過校正的導航結果； 述高度測量裝置將所述平均海平面以上的^度測量值發 所 送給卡爾曼濾波器者

   3 9 ·如專利申请範圍第3 8項所述之一增強型導航定位系 統，其中該INS處理器包括一IMUI/〇接口，一ΙΜϋ誤差補償 模塊，一坐標轉換計算模塊，一姿態位置速度計算模塊^ /轉換矩陣计异模塊，和一地球和運動體轉動速率計算模 塊，其中該IMUI/0接口從該ΙΜϋ接收該機體轉動角速率和 比力信號，並處理轉化為數字數據，這些數字數據迭加加 有該慣性傳感器測量誤差，該受慣性傳感器測量誤差污染 的慣性測量值送給該IMU誤差補償模塊，其中該ΙΜϋ誤差補 償模塊接收來自該卡爾曼濾波器的傳感器誤差估計值，並 執行IMU誤差校正，該校正後的慣性數據送給該坐標轉換 計^模塊和該轉換矩陣計算模塊，該比力送給該坐標轉換 計算模塊，該轉換矩陣計算模塊從該IMU誤差計算模塊接 收該機體角速率，從該地球和運載體轉動速率計算模塊接 ，地球和運載體轉動角速率，並執行轉換矩陣的計算，

   忒轉換矩陣計算模塊將該轉換矩陣送給該坐標轉換計算模 ，和，姿態，置速度計算模塊，該轉換矩陣計算模塊中的 一姿態更新算法利用四元素法進行姿態計算，其中該坐標 轉換，塊從該IMU誤差計算模塊接收該比力，從該轉動矩 陣计$模塊接收轉動矩陣，並執行該坐標轉換，該坐標轉 換什异模塊將轉換到由該轉換矩陣表徵的坐標系中的比力 送給邊姿態位置速度計算模塊，其中該姿態位置速度計算

   第79頁 531657 六、申請專利範圍 模塊接收從該坐標轉換計算模塊接收該轉換後的比力，從 該轉換矩陣計算模塊接收該轉換矩陣，並更新該姿態’位 置，和速度者。 4 0 ·如專利申請範圍第3 9項所述之一增強型導航定位系 統，其中在進行該位置和速度計算之後，由卡爾曼濾波器 計算得來的該位置和速度誤差用在該姿態位置速度計算模 塊中來校正該慣性解者。 4 1 ·如專利申請範圍第4 0項所述之一增強型導航定位系 統，其中由該卡爾曼濾波器計算得出的姿態誤差送給該姿 態位置速度計算模塊，並在該姿態位置速度計算模塊中執 行姿態校正者。 42·如專利申請範圍第4〇項所述之一增強型導航定位系 統’其中由該卡爾曼濾波器計算得出的姿態誤差送給該轉 換矩陣計算模塊，並在姿態位置速度計算模塊之前執行姿 態校正者。 43·如專利申請範圍第41、或42項所述之一增強型導航定 位系統，其中由該姿態位置速度計算模塊得來的校正後的 ，性解送給該卡爾曼濾波器來建立該卡爾曼濾波器的觀測 量，同時校正後的慣性解送給該載波相位整周模糊求解模 塊來輔助求解全球定位系統衛星信號載波相位整周模糊 值’校正後的速度和加速度信息送給GPS處理器的該微處 理器來輔助全球定位系統衛星信號的載波相位和碼跟蹤， 其中由姿態位置速度計算模塊計算得來的速度送給該地球 和運載體轉動速率計算模塊來計算地球轉動速率和運載體

   531657

   ^動速帛，$地球轉動冑率和冑載體轉動料送給該轉換 陣计异模塊，其中該姿態，位置和速度信息送給該丨/〇 接口者。 44·如專利申請色圍第43項所述之增強型導航定位系統， 其中所述卡爾曼濾波器是一強健型卡爾曼濾波器，能夠為 田系列混看所述全球定位系統量測值和所述慣性傳感器測 量得過程和模型提供進於優化的濾波者。 4 5 ·如專利申清範圍第4 4項所述之一增強型導航定位系 統，其中該穩健卡爾曼濾波器包括一 GPS誤差補償模塊用 於從該GPS處理器接收該gps觀測量，包括該偽距，載波相 位和多卜勒頻率，從一狀態向量更新模塊接收位置和速度 校正量，並執行GPS誤差補償以生成校正過的GPS原始數 據’並將該G P S原始數據送給一預處理模塊，該預處理模 塊從該GPS處理器接收gps衛星星歷數據，從該GPS誤差補 償模塊接收該校正後的GPS原始數據，從該高度測量裝置 接收該運載體高度觀測量，從該I NS處理器接收丨NS導航 解，該預處理模塊計算狀態傳輸矩陣，並將該狀態傳輸矩 陣連同該狀態向量送給一狀態向量預測模塊，該計算得到 的狀態傳輸矩陣也送給一協方差傳播模塊，該協方差傳播 模塊根據一計算得到的觀測矩陣和一觀測模型計算一觀測 矩陣和當前觀測向量，該觀測矩陣和該當前觀測向量送給 一觀測量殘差計算模塊，該狀態向量預測模塊從該預處理 模塊接收該狀態傳輸矩陣和該狀態向量，並對當前時刻的 狀悲進彳于預測’該預測的當前狀恕向置送給該觀測量殘差

   第81頁

   2002· 12· 13. 081 531657

   估計值，包括位置，速度和姿態誤差，該位置，速度和姿 恶誤差送給該G P S誤差補償模塊和該I N S處理器者。 4 6 · —種增強型導航定位系統包括： 計算模塊， 接收預測的 陣和該當前 前觀測向量 積來計算觀 計算模塊來 該殘差監視 收狀態傳輸 的協方差， 塊，該最優 誤差的當前 協方差更新 塊更新該估 給該協方差 計算模塊接 該觀測量殘 該觀測量殘 當前狀態向 觀測向量， 減去該觀測 測量殘差， 的觀測量殘 模塊接收系 矩陣和估計 該估計誤差 增益計算模 協方差’並 模塊和該狀 計誤差的協 傳播矩陣， 收該最優增 差，該狀態 差計算模塊 量’從該預 該觀測量殘 矩陣與該當 該殘差監視 差進行判別 、统過程的協 誤差的協方 的協方差送 塊從該協方 計算最優增 態向量更新 方差，並將 該狀態向量 益，從該觀 向量更新模 從該狀態向 處理模塊接 差計算模塊 前狀態向量 模塊對從該 ’該協方差 方差’從預 差來計算當 給一最優增 差計算模塊 益，該最優 模塊，該協 該估計誤差 更新模塊從 測量殘差計 塊計算當前 量預測模塊 收該測量矩 通過從該當 預測值的乘 觀測量殘差 傳播模塊從 處理模塊接 前估計誤差 益計算模 接收該估計 增益送給一 方差更新模 的協方差送 該最優增益 算模塊接收 狀態向量的 一全球定位系統GPS處理器用於提供與位置有關的信 6 · ^ ， 一慣性測量單元IMU用於提供慣性觀測量，包括載體 的轉動角速率和比力； 一高度觀測量產生器用於提供載體高度觀測量；其中

   第82頁

   々、申讀專利範圍 /'所述高度測量裝置是 531657 而的高度； α /中心導航處理器，它與該GPS處理器’該1MU和該高 许覷測量產生器相連，並用於融合來自該GPS處理器的該 =位置有關的信息，來自該1MU的該慣性觀測量和來自該 /高度觀測量產生器的該高度觀測量來產生載體的導航解， a姑位置，速度，和姿態； ° 一輸入/輸出（I/O)接口，它與該中心導航處理器相 連，用於輸出該導航解； 立中所述全球定位系統處理器提供全球定位系統位置數 i，其中所述中心導航處理器連接所述全球定位系統處理 器，所述慣性測量單元，所述雷達高度計’所述中央導航 ^理器，包括一個慣性導航處理器，一個數據融合模塊， 二個地形地貌數據庫，以及一個卡爾曼濾波器。所述全球 定位系統位置數據被傳送到中央導航處理器’雷達高度計 的輸出被傳送到所述數據融合模塊，所述慣性測量數據被 傳送到慣性導航系統處理器；慣性導航系統處理器的輸 出，數據融合模塊的輸出，以及全球定位系統位置數據在 卡爾曼濾波器中被混合，而所述卡爾曼濾波器的輸出則被 反饋到所述慣性導航系統處理器中用來校正從所述中央航 行處理器輸出到戶斤述I / 〇界面的一個慣性導航系統，纟口果， 所述全球定位處理器的微處理器輸出所述全球定位系 統位置數據，全球定位系統星歷，以及大氣參數給所述卡 爾曼濾波器；

   第83頁 531657 六、申請專利範圍 所述慣性導航系統處理器處理所述慣性量測值，包括 體角速度，外力，以及從卡爾曼濾波器得到的方位誤差， 速度誤差，高度誤差以推導經過校正的導航結果；

   所述雷達高度計將所述運動器高度數據發送給所述數 據融合模塊，所述慣性導航系統處理器將所述運動器位置 數據發送給所述地形地貌數據庫，所述地形地貌數據庫進 行數據查詢以獲得地表在平均海平面以上的高度數據並將 其發送給數據融合模塊，所述數據融合模塊從雷達高度計 獲得運動器在地表以上的高度，從地形地貌數據庫獲得地 表在平均海平面以上的高度，並推導出運動器在平均海平 面以上的高度並將其發送給卡爾曼濾波器者。 4 7 ·如專利申請範圍第4 6項所述之一增強型導航定位系 統’其中該INS處理器包括一IMUI/0接口，一IMU誤差補償 模塊，一坐標轉換計算模塊，一姿態位置速度計算模塊， 一轉換矩陣計算模塊，和一地球和運動體轉動速率計算模 塊，其中該IMU I/O接口從該IMU接收該機體轉動角速率和 比力信號，並處理轉化為數字數據，這些數字數據迭加加 有該慣性傳感器測量誤差，該受慣性傳感器測量誤差污染 的慣性測量值送給該IMU誤差補償模塊，其中該IMU誤差補 償模塊接收來自該卡爾曼濾波器的傳感器誤差估計值，並 執行IMU誤差校正，該校正後的慣性數據送給該坐標轉換 計算模塊和該轉換矩陣計算模塊，該比力送給該坐標轉換 計算模塊，該轉換矩陣計算模塊從該IMU誤差計算模塊接 收該機體角速率，從該地球和運載體轉動速率計算模塊接

   第84頁 531657

   收一地 該轉換 塊和該 一姿態 轉換模 陣計算 換計算 送給該 模塊接 球和運載體轉動角速率，並热 矩陣計算模塊將該轉換矩陣矩陣的計算， ί;算ϊΐ ’該轉換矩陣計算模塊中的 更新异法利用四兀素法進行姿態計算， ？ 塊從該IMU誤差計算模塊接收該比力，從該轉動矩τ 模塊接收轉動矩陣，並執行該坐標轉換，該坐標轉 模塊將轉換到由該轉換矩陣表徵的坐標系中的比力 姿態位置速度計算模塊，其中該姿態位置速度計算 收從該坐標轉換計算模塊接收該轉換後的比力，從 該轉換矩陣計算模塊接收該轉換矩陣，並更新該姿態，位 置，和速度者。 48.如專利申請範圍第47項所述之一增強型導航定位系 統’其中在進行該位置和速度計算之後，由卡爾曼濾波器 計算得來的該位置和速度誤差用在該姿態位置速度計算模 塊中來校正該慣性解者。 4 9·如專利申請範圍第48項所述之一增強型導航定位系 統’其中由該卡爾曼濾波器計算得出的姿態誤差送給該姿 態位置速度計算模塊，並在該姿態位置速度計算模塊中執 行姿態校正者。 5 0 ·如專利申請範圍第4 8項所述之一增強塱導航定位系 統，其中由該卡爾曼滤波器計算得出的姿態誤差送給該轉 換矩陣計算模塊，並在姿態位置速度計算模塊之前執行姿 態校正者。 51·如專利申請範圍第項、或第50項所述之增強型導

   第85頁 531657 六、申請專利範圍 " 航疋位系統，其中由該姿態位置速度計算模塊得來的校正 後的性解送給該卡爾曼濾波器來建立該卡爾曼濾波器的 觀測Η ’同時校正後的慣性解送給該載波相位整周模糊求 解模塊來輔助求解全球定位系統衛星信號載波相位整周模 糊值’校正後的速度和加速度信息送給GPS處理器的該微 處理器來輔助全球定位系統衛星信號的載波相位和碼跟 蹤’其中由姿態位置速度計算模塊計算得來的速度送給該 地球和運載體轉動速率計算模塊來計算地球轉動速率和運 載體轉動速率，該地球轉動速率和運載體轉動速率送給該 轉換矩陣计算模塊，其中該姿態’位置和速度信息送給該 I /0接口者。 52·如專利申請範圍第50項所述之一增強型導航定位系 統’其中所述卡爾曼濾波器是一強健型卡爾曼濾波器，能 夠為一系列混淆所述全球定位系統量測值，所述高度測量 裝置，以及所述慣性傳感器測量得過程和模型提供進於優 化的渡波者。 5 3·如專利申請範圍第52項所述之一增強型導航定位系 統，其中該穩健卡爾曼濾波器包括一GPS誤差補償模塊用 於從該GPS處理器接收該GPS觀測量，包括該偽距，載波相 位和多卜勒頻率，從一狀態向量更新模塊接收位置和速度 校正量，並執行GPS誤差補償以生成校正過的GPS原始數 據，並將該GPS原始數據送給一預處理模塊，該預處理模 塊從該GPS處理器接收GPS衛星星歷數據，從該GPS誤差補 償模塊接收該校正後的GPS原始數據，從該高度測量裝置

   第86頁 531657

   接收該運載體高度觀測量， 解，該預處理模塊計算狀態 陣連同該狀態向量送給一狀 的狀態傳輸矩陣也送給一協 模塊根據一計算得到的觀測 矩陣和當前觀測向量，該觀 一觀測量殘差計算模塊，該 模塊接收該狀態傳輸矩陣和 狀態進行預測，該預測的當 計算模塊，該觀測量殘差計 接收預測的當前狀態向量， 陣和该當前觀測向量，該觀 剷觀測向量減去該觀測矩陣 積來計算觀測量殘差，該殘 计异模塊來的觀測量殘差進 該殘差監視模塊接收系統過 收狀態傳輸矩陣和估計誤差 的協方差，該估計誤差的協 塊，該最優增益計算模塊從 誤差的當前協方差，並計算 協方差更新模塊和該狀態向 塊更新該估計誤差的協方差 給該協方差傳播矩陣，該狀 計算模塊接收該最優增益， 從該1Ns處理器接收INS導航 傳輸矩陣，並將該狀態傳輸矩 態向量預測模塊，該計算得到 方差傳播模塊，該協方差傳播 矩陣和一觀測模型計算一觀測 測矩陣和該當前觀測向量送給 狀態向量預測模塊從該預處理 該狀態向量，並對當前時刻的 前狀態向量送給該觀測量殘差 异模塊從該狀態向量預測模塊 從該預處理模塊接收該測量矩 測量殘差計算模塊通過從該當 與該當前狀態向量預測值的乘 差監視模塊對從該觀測量殘差 行判別，該協方差傳播模塊從 程的協方差，從預處理模塊接 的協方差來計算當前估計誤差 方差送給一最優增益計算模 該協方差計算模塊接收該估計 最優增益，該最優增益送給一 量更新模塊，該協方差更新模 ，並將該估計誤差的協方差送 態向量更新模塊從該最優增益 該觀測量殘差計算模塊接收

   第87頁 531657 六、申請專利範圍 該觀測量殘差，該狀態向量更新模塊計算當前狀態向量的 估計值，包括位置，速度和姿態誤差，該位置，速度和姿 態誤差送給該GPS誤差補償模塊和該INS處理器者。

   第88頁