多介質復雜環境下高精度視覺/慣性組合導航方法
【專利說明】多介質復雜環境下高精度視覺/慣性組合導航方法 【技術領域】
[0001] 本發明為多介質復雜環境下高精度視覺/慣性組合導航方法，屬于組合導航技術 領域。 【【背景技術】】
[0002] 近年來，在地面模擬空間的微重力環境是進行空間飛行器地面試驗的重要組成部 分，而利用水下航行器模擬空間微重力環境中飛船衛星的交會對接，為空間交會對接提供 重要的測量數據和試驗驗證。西北工業大學飛行器設計國家重點實驗室建立了相應的試驗 系統，在地面進行水下航行器的自主交會對接，通過水的浮力和調整地面電磁場的方向和 磁場強度，使得水下航行器處于中性浮力水平，從而滿足天地一致性的等效試驗條件。而完 成交會對接任務的一個重要前提是高精度的導航系統提供精確的測量信息。
[0003] 目前，常用的導航方式有：諸如慣性導航、視覺導航、GPS的衛星導航、多普勒雷達 導航等單系統導航方式，以及里程計/INS組合導航、GNSS/INS組合導航、多普勒雷達/INS 組合導航系統等[1]。然而，在諸如室內、水下等環境中，GPS導航無法提供有效的導航信 息。在強電磁等復雜中，多普勒雷達等無線電導航方式也將失效。因此，當載體運動處于多 介質復雜環境下時，很多導航方法并不能實施。
[0004] 慣性導航系統（Inertial Navigation System, INS)由慣性測量單元（Inertial Measurement Unit, IMU)、采集裝置、導航解算軟件和導航計算機組成，具有自主性、全天候 工作、導航信息全等優點。但由于其導航方式為遞推方式，誤差積累會造成導航結果發散， 并且在復雜電磁場環境中會加劇這種發散，因此需盡量消除復雜電磁場環境對MU輸出數 據的影響，但限于目前MU的材料和工藝，并不能完全隔離電磁場環境對其的影響，可以利 用算法補償的方法消除電磁場的影響。在需要長航時導航時，除了隔離復雜電磁場環境影 響外，INS必須與誤差不積累的導航系統進行組合才能提供高精度的導航信息。
[0005] 視覺導航系統（Vision Position System, VPS)由1個高速雙目CCD相機、采集裝 置、系統測量軟件、工控機組成。通過雙目CCD相機進行目標圖像采集，通過采集裝置將相 機采集到的圖像傳輸到工控機，系統測量軟件對相機的圖像進行處理分析，構建各特征點 的成像光路圖，利用多介質雙目測量原理計算出目標航行器各特征點的三維空間坐標值， 進而計算出運動體的三維空間位姿測量值[2] [3]。
[0006] VPS系統的主要誤差來源為圖像處理誤差和光噪聲誤差等，誤差不隨時間積累，在 室內、水下、陸上和空間等介質中均可應用，并且其測量不受電磁場等環境的影響，因此，將 VPS和INS進行組合導航，為多介質復雜環境下的載體提供高精度視覺/慣性組合導航方 法。
[0007] 參考文獻
[0008] [1]張仁勇，羅建軍，馬衛華，蘇二龍.交會對接視覺相對導航系統半物理仿真 [J] ·計算機仿真，2012, 06:75-79.
[0009] [2]王俊，朱戰霞，賈國華，張旭陽.多介質下空間目標的視覺測量[J].計算機 應用，2011，05:1431-1434.
[0010] [3] Li M, Mourikis A 1.0 ptimization-Based Estimator Design for Vision-Aided Inertial Navigation[C]. Robotics: Science and Systems, 2012.
[0011] [4]張昊，石磊，涂俊峰，管樂鑫，解永春.基于交會對接C⑶光學成像敏感器 的雙目測量算法[J].空間控制技術與應用，2011，06:66-71. 【
【發明內容】
】
[0012] 本發明的目的是提出一種多介質復雜環境下高精度視覺/慣性組合導航方法。本 方法適用于透明介質內復雜電磁環境中水下航行器的組合導航，為多介質復雜環境下的水 下航行器提供高精度的視覺/慣性導航方法。
[0013] 一種多介質復雜環境下高精度視覺/慣性組合導航裝置，適用于透明介質內復雜 電磁環境中水下航行器的組合導航，包括水下航行器，該水下航行器放置在透明介質的容 器內，該容器底部放置有電磁鐵陣列，水下航行器的四周均安裝有一個推進器，在其中的一 個推進器前端安裝有用于抓取的對接機構，所述水下航行器的正下方安裝有一個永磁體， 用以調節水下航行器實現中性浮力水平；所述水下航行器內裝有慣性導航系統，該慣性導 航系統內設置有慣性測量單元，所述慣性導航系統通過光纖完成數據和指令的傳輸；在所 述透明介質的外圍設置有視覺導航系統，該視覺導航系統包括有雙目CCD相機。
[0014] -種基于所述裝置的多介質復雜環境下高精度視覺/慣性組合導航方法，利用水 下航行器模擬空間微重力環境中飛船衛星的交會對接，為空間交會對接提供重要的測量數 據和試驗驗證；整個實驗系統在地面進行，通過水的浮力，地面電磁場的方向和磁場強度的 調整，使得水下航行器處于中性浮力水平，從而滿足天地一致性的等效試驗條件。
[0015] 所述組合導航系統，具體包括以下步驟：
[0016] (1)采用雙目CCD相機對水下航行器進行圖像采集，實現水下航行器位置的測量；
[0017] (2)建立電磁場強度與慣性測量單元中陀螺測得的角增量之間的關系；建立電磁 場強度與慣性測量單元中加速度計測得的速度增量之間的關系；
[0018] (3)根據實時測量的電磁場強度對陀螺和加速度計的測量數據進行補償；
[0019] (4)對視覺導航系統的狀態進行判斷，若發生異常，則對視覺導航系統得到的位置 信息進行隔離，若正常，則對信息進行處理并將時鐘信息傳遞給慣性導航系統；
[0020] (5)對慣性導航系統的狀態進行判斷，若異常，則對慣性導航系統得到的數據進 行隔離，若正常，則根據視覺導航系統的時標信息對慣性導航系統進行時間同步和數據擬 合；
[0021] (6)慣性導航系統根據步驟（5)數據擬合的結果進行自適應周期的導航解算，輸 出水下航行器相對于當地地理坐標系下的姿態、速度、位置和加速度信息；
[0022] (7)視覺導航系統根據步驟（4)的結果輸出水下航行器相對于視覺坐標系的位置 信息；
[0023] (8)若視覺導航系統和慣性導航系統的工作狀態均正常，則將慣性導航系統和視 覺導航系統的結果統一到導航坐標系，然后轉入步驟（9)進行組合導航模式；
[0024] (9)根據慣性導航系統的姿態誤差角方程、速度誤差方程、位置誤差方程、陀螺和 加速度計誤差模型以及視覺導航系統的位置誤差方程建立組合導航卡爾曼濾波器的狀態 方程，根據慣性導航系統與視覺導航系統的位置差建立組合導航系統卡爾曼濾波器的量測 方程，根據導航時間選擇輸出校正或反饋校正的組合導