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