一種復雜多介質(zhì)環(huán)境下的視覺/慣性自適應組合導航方法

張朝飛，羅建軍，龔柏春，朱戰(zhàn)霞

（1. 西北工業(yè)大學 航天學院，西安 710072；2. 航天飛行動力學技術(shù)重點實驗室，西安 710072）

一種復雜多介質(zhì)環(huán)境下的視覺/慣性自適應組合導航方法

張朝飛1,2，羅建軍1,2，龔柏春1,2，朱戰(zhàn)霞1,2

（1. 西北工業(yè)大學 航天學院，西安 710072；2. 航天飛行動力學技術(shù)重點實驗室，西安 710072）

針對空間交會對接地面混合懸浮微重力模擬實驗中自主水下航行器的高精度實時導航問題，提出了一種復雜多介質(zhì)環(huán)境下視覺/慣性自適應組合導航方案和算法。首先，以視覺定位系統(tǒng)的采樣時標為基準對慣導系統(tǒng)的測量數(shù)據(jù)進行擬合，并通過慣導逆向?qū)Ш降姆椒▽崿F(xiàn)慣導與視覺定位系統(tǒng)的時間同步。然后，根據(jù)視覺系統(tǒng)量測噪聲時變的特點，設(shè)計了可對量測噪聲在線估計的指數(shù)漸消記憶型Sage-Husa自適應卡爾曼濾波組合導航算法。最后，通過實驗驗證了該組合方案和算法的有效性。實驗結(jié)果表明提出的方法可以實現(xiàn)復雜多介質(zhì)環(huán)境下的高精度實時導航，并且相比于測量數(shù)據(jù)未同步的標準卡爾曼濾波組合導航方式，其速度誤差和位置導航誤差分別降低了56%和64% 。

視覺定位系統(tǒng)；慣性導航系統(tǒng)；同步；自適應卡爾曼濾波；組合導航

利用自主式水下航行器(Autonomous Underwater Vehicle, AUV)進行地面實驗，模擬空間微重力環(huán)境中飛船衛(wèi)星的交會對接，為空間交會對接提供重要的測量數(shù)據(jù)和實驗驗證，具有重要的實際應用價值。通過調(diào)整中性浮力設(shè)施，使AUV處于懸浮狀態(tài)，從而滿足天地一致性的等效試驗條件。控制AUV模擬空間交會對接，需要對其姿態(tài)、速度和位置信息進行準確測量[1-2]。由于工作在水下，一些常用的測量方式受到限制而不能使用，如紅外跟蹤定位儀由于透明介質(zhì)的折射無法實現(xiàn)AUV的精準定位，GPS由于信號無法在水中傳播也無法使用[3]。文獻[4-5]提出利用視覺定位系統(tǒng)（Visual Positioning System, VPS）進行位置測量，VPS具有低成本、低功耗和高可靠性的顯著優(yōu)點，已經(jīng)被廣泛的應用在地面機器人、無人機和水下機器人的導航系統(tǒng)中。但VPS也有以下缺點：1）雙目CCD視覺系統(tǒng)經(jīng)過標定后僅能提供精確的位置信息，導航信息單一；2）由于VPS的定位依賴于圖像處理，受使用環(huán)境、CCD相機拍攝圖像的質(zhì)量和載體運動快慢等因素的影響，信息的更新頻率低且不固定，不利于實時控制[6]。慣性導航系統(tǒng)（INS）能夠以固定的頻率輸出載體相對于導航坐標系的姿態(tài)、速度和位置信息，但由于其自身誤差積累的特點，不適合單獨進行長時間導航。針對以上問題，本文依據(jù)VPS和INS的特點進行信息融合，提出多介質(zhì)環(huán)境下高精度視覺/慣性自適應組合導航方法。通過AUV與目標體的模擬交會對接過程驗證了組合導航方法的有效性。

1 多介質(zhì)環(huán)境下視覺/慣性組合導航系統(tǒng)的組成及原理

空間交會對接地面混合懸浮微重力模擬實驗系統(tǒng)見圖1，由AUV、慣性測量單元（Inertial Measurement Unit, IMU）、雙目CCD相機、上位機和玻璃水池組成。裝有IMU的AUV放置在裝滿水的玻璃水池中。雙目CCD相機透過玻璃和水對AUV進行觀測，IMU實時測量 AUV的角運動和線運動信息，通過通信光纖將陀螺和加速度計的數(shù)據(jù)實時傳輸至上位機，根據(jù)初始位置信息和當?shù)刂亓铀俣?，在上位機上進行實時的導航解算，提供AUV在當?shù)氐乩碜鴺讼担ㄒ訟UV中心為原點，X-Y-Z指向東-北-天）下的姿態(tài)、速度和位置信息。

圖1 空間交會對接地面混合懸浮微重力模擬實驗系統(tǒng)Fig.1 Experiment system of spacecraft rendezvous and docking under multi-medium environment

雙目CCD相機作為VPS的測量元件，需要經(jīng)過標定才能實現(xiàn)精確的定位功能。通過對雙目 CCD相機進行標定，確立左右相機的位置關(guān)系，建立VPS視覺坐標系（以左相機安裝位置為原點，X-Y-Z指向東-北-天），采用雙目CCD相機對AUV進行圖像采集。上位機對采集到的數(shù)據(jù)進行處理，根據(jù)光線在空氣、透明介質(zhì)和水中的折射率建立折射光線的路徑方程，通過多介質(zhì)傳輸路徑補償，消除空氣、透明介質(zhì)和水的折射對AUV位置觀測的影響，實現(xiàn)AUV位置的精確測量。

VPS的原理圖見圖2，OL和OR分別為左相機和右相機的光心，pL和pR分別為P點在左相機和右相機上的成像點，GL和GR分別為P點在折射面1上的折射點分別為P點在折射面2上的折射點，和αR分別為由折射面1到相機的單位向量，′和分別為光線經(jīng)過一次折射之后的單位向量，ωL和分別為光線經(jīng)過二次折射之后的單位向量，αL和 αR分別為左相機和右相機中光線的入射角，βL和βR分別為左相機和右相機中光線的一次折射角， γL和γR分別為左相機和右相機中光線的二次折射角，n1、和n3分別為光線在空氣、玻璃和水中的折射率，d為玻璃的厚度。AUV的位置坐標由式(1)計算得到[4]：

圖2 視覺定位系統(tǒng)原理圖Fig.2 Schematic of visual positioning system

2 周期自適應的視覺/慣性同步方法

組合導航系統(tǒng)的流程見圖3。對CCD和IMU的狀態(tài)進行實時監(jiān)測，若任一部分出現(xiàn)異常，則對相應的信息進行隔離。若二者工作狀態(tài)正常，按照圖3流程進行相應的處理。

由于VPS信息更新頻率不固定，而IMU的輸出頻率固定，二者在進行組合時，若不考慮同步問題，在VPS信息更新時與IMU最新時刻的輸出進行組合，必然造成組合導航系統(tǒng)的精度損失[7-10]。因此，本文在不增加硬件的前提上，提出了以 CCD相機拍攝時刻為基準，對IMU陀螺和加速度計的輸出增量信息進行數(shù)據(jù)擬合。當VPS信息更新時INS進行逆向?qū)Ш?，在CCD相機的拍攝時刻進行組合導航，從而實現(xiàn)VPS與INS的時間同步。具體過程如下：

圖3 組合導航系統(tǒng)流程圖Fig.3 Flow chart of integrated navigation system

CCD相機與IMU同時啟動，CCD相機采集圖像和VPS信息更新由上位機的時鐘計時，IMU時鐘由其內(nèi)部晶振提供，陀螺和加速度計以固定頻率輸出。CCD相機拍攝和VPS信息更新時自動記錄當前時間信息并傳遞給INS，同步原理圖見圖4。由于IMU的采樣頻率較高（一般為200 Hz或更高），陀螺和加速度計的輸出建立時間和輸出的線性內(nèi)插模型，擬合出陀螺和加速度計的自CCD相機拍攝時刻到IMU下一個采樣時刻的增量輸出信息，如式(2)所示：

圖4 INS與VPS同步原理圖Fig.4 Synchronous schematic of INS and VPS

首先INS進行導航解算。當CCD相機拍攝AUV圖片時，上位機自動記錄 CCD相機的拍攝時刻，根據(jù)式(2)計算對應的增量信息。INS進行周期自適應的導航解算，實時輸出AUV的姿態(tài)、速度和位置信息。當VPS信息更新時，上位機自動記錄其更新時刻。然后依據(jù)存儲的陀螺、加速度計和 CCD相機拍攝時刻所對應到IMU采樣周期內(nèi)的時刻t進行周期自適應的逆向?qū)Ш?，利用逆向?qū)Ш酵瓿蓵r的姿態(tài)、速度取反和位置信息與 VPS的位置信息進行組合導航，對 INS的誤差進行修正。然后根據(jù)存儲的正向陀螺和加速度計采樣數(shù)據(jù)進行周期自適應的INS導航解算，實時輸出VPS信息更新時刻之后AUV相對于當?shù)氐乩碜鴺讼迪碌淖藨B(tài)、速度和位置信息。

逆向?qū)Ш降倪^程如下：陀螺和加速度計采樣數(shù)據(jù)進行存儲后按時間進行正向和逆向排列，形成正向序列和逆向序列。由正向?qū)Ш降慕馑愎酵茖С瞿嫦驅(qū)Ш降慕馑愎?。假設(shè)正向?qū)Ш降闹芷跒閚個，以第n個周期的姿態(tài)、速度取反和以位置作為逆向?qū)Ш降某跏甲藨B(tài)、初始速度和初始位置，將陀螺采樣和地球自轉(zhuǎn)角速率取反，并且對陀螺和加速度計的采樣數(shù)據(jù)進行逆序處理，形成逆向數(shù)據(jù)序列。若不考慮計算誤差，逆向?qū)Ш綍r各時刻的姿態(tài)信息、位置信息與正向?qū)Ш皆趯獣r刻上的姿態(tài)信息、位置信息相同，速度信息大小相同，方向相反。由上述推導可知，正向?qū)Ш胶湍嫦驅(qū)Ш降慕馑闶较嗤虼似湎到y(tǒng)誤差方程也相同，逆向?qū)Ш降姆匠膛c正向?qū)Ш降姆匠掏耆嗤?，僅導航過程中的輸出信息不同[11]。

3 指數(shù)漸消記憶的 Sage-Husa自適應卡爾曼濾波組合導航算法

當INS逆向?qū)Ш酵瓿蓵r，姿態(tài)、速度取反和位置信息經(jīng)過坐標變換后與經(jīng)過坐標變換后的 VPS的位置信息進行組合導航（本文選取水池坐標系為導航坐標系，以水池西南角底部為原點，X-Y-Z指向東-北-天）。根據(jù) INS的姿態(tài)誤差角方程、速度誤差方程、位置誤差方程以及陀螺和加速度計誤差方程建立卡爾曼濾波（Kalman Filter, KF）組合導航的狀態(tài)方程，根據(jù)INS與VPS的位置差建立量測方程。由于VPS數(shù)據(jù)采集頻率不固定，在 VPS不更新時僅進行狀態(tài)更新，在VPS更新時進行量測更新。

視覺/慣性組合導航系統(tǒng)的狀態(tài)方程用連續(xù)微分方程描述，其狀態(tài)方程和量測方程分別為

根據(jù)線性系統(tǒng)理論對式(3)進行離散化，離散化后的狀態(tài)方程為

在利用AUV進行交會對接過程的實時組合導航中，系統(tǒng)的狀態(tài)噪聲主要由IMU中慣性器件自身的隨機誤差引起，這些隨機誤差可以通過標定的方法進行建模。而VPS的定位精度受環(huán)境光線、AUV的運動速率快慢以及晃動幅度大小等條件的影響，導致系統(tǒng)的量測噪聲具有隨機性和時變性，故對量測噪聲的特性不能事先準確建模。標準的KF算法在組合導航建模時，只有在系統(tǒng)的狀態(tài)參數(shù)和噪聲統(tǒng)計特性參數(shù)都準確已知的前提下才能達到估計狀態(tài)的最優(yōu)。由于組合導航系統(tǒng)量測噪聲的統(tǒng)計特性不能事先確定，可能導致標準KF的精度降低，嚴重時甚至引起濾波發(fā)散。

Sage-Husa自適應卡爾曼濾波（Sage-Husa Adaptive Kalman Filter, SHAKF）可以通過時變噪聲估計器對量測噪聲的統(tǒng)計特性進行實時統(tǒng)計和修正，達到降低模型誤差，提高濾波精度和抑制濾波發(fā)散的目的。SHAKF在噪聲參數(shù)的估計時進行等加權(quán)時間平均的自適應估計，當已知噪聲參數(shù)為固定值時，在估計有效和收斂的前提下，噪聲參數(shù)的估計隨著時間的增長會越來越準確。但實際系統(tǒng)中，若系統(tǒng)的噪聲參數(shù)發(fā)生緩慢變化，加權(quán)平均法的估計值必然引入滯后效應，估計值不能及時地反映噪聲參數(shù)的變化。而指數(shù)漸消記憶的SHAKF可以對系統(tǒng)量測噪聲參數(shù)發(fā)生的變化動態(tài)進行估計，其實質(zhì)是在估計的過程中根據(jù)噪聲參數(shù)變化的程度設(shè)定特定的遺忘因子，對早期數(shù)據(jù)的影響進行有效衰減[12-13]。由于 AUV運動過程中會引起水的波動，導致系統(tǒng)量測噪聲參數(shù)發(fā)生緩慢變化，系統(tǒng)的量測噪聲的參數(shù)也會隨之發(fā)生變化。

因此，本文采用指數(shù)漸消記憶的SHAKF算法設(shè)計組合導航算法，具體過程如下：

4 實驗驗證

利用空間交會對接地面混合懸浮微重力模擬實驗系統(tǒng)完成AUV和目標體的交會對接實驗，調(diào)整浮力系統(tǒng)使 AUV處于懸浮狀態(tài)模擬微重力環(huán)境?？刂葡到y(tǒng)通過通信光纖完成 IMU數(shù)據(jù)和推進器的控制指令的傳輸，雙目CCD相機安裝于距水池1.5 m的地方。

圖5 交會對接軌跡圖Fig.5 Trajectory of rendezvous and docking

圖6 組合導航速度誤差Fig.6 Velocity error of integrated navigation

圖7 組合導航位置誤差Fig.7 Position error of integrated navigation

AUV按照圖5中標定過的軌跡進行運動，實驗過程中的計算條件和技術(shù)參數(shù)見表1。

分別利用未同步的標準KF組合導航方法與本文提出的同步后的指數(shù)漸消記憶SHAKF組合導航方法進行比較。實驗結(jié)果如圖6和圖7所示，可以看出未同步的標準KF算法的速度誤差和位置誤差較大，其中最大速度誤差為0.09m/s，最大位置誤差為0.39m，且位置誤差出現(xiàn)明顯的發(fā)散趨勢。同步后的指數(shù)漸消記憶的SHAKF算法的速度誤差和位置誤差均收斂，其中速度誤差最大為0.04m/s，位置誤差最大為0.14m。組合導航后位置誤差比VPS的誤差大是因為VPS的數(shù)據(jù)更新時 AUV的光標被機械臂遮擋導致VPS的輸出出現(xiàn)較大的跳變。

對比圖6和圖7可知，同步后的指數(shù)漸消記憶的SHAKF組合導航方法相較于未同步的標準KF組合導航方法速度誤差減小了56%，位置誤差減小了64%，并且同步后的指數(shù)漸消記憶的SHAKF算法組合導航算法穩(wěn)定性也有所提高。

表1 實驗條件及部分重要參數(shù)Tab.1 Experimental conditions and important parameters

5 總 結(jié)

1）本文提出的多介質(zhì)環(huán)境下視覺/慣性的自適應組合導航的方法，通過INS擬合CCD相機拍攝時刻的增量輸出，進行逆向?qū)Ш胶髮崿F(xiàn)與 VPS的時間同步，減小了由于時間不同步造成的導航誤差，避免了時間不同步造成的組合導航發(fā)散問題.

2）利用指數(shù)漸消記憶的SHAKF算法進行組合導航，能夠在系統(tǒng)量測噪聲統(tǒng)計特性未知且緩慢變化的環(huán)境下，增強濾波算法的穩(wěn)定性。

3）通過在地面混合懸浮微重力環(huán)境中模擬空間交會對接實驗，驗證了本文提出的方法具有為地面實驗實時提供高精度導航信息的能力。

（References）：

[1] Kelsey J M, Byrne J, Cosgrove M, et al. Vision-based relative pose estimation for autonomous rendezvous and docking[C]//2006 IEEE Aerospace Conference. 2006: 1333-1352.

[2] D’Amico S, Ardaens J S, Gaias G, et al. Noncooperative rendezvous using angles-only optical navigation: system design and flight results[J]. Journal of Guidance Control & Dynamics, 2013, 36(6): 1576-1595.

[3] Smithanik J R, Atkins E M, Sanner R M. A visual positioning system for an based underwater space simulation environment[J]. Journal of Guidance, Control, and Dynamics, 2006, 29(4): 858-869.

[4] 王俊, 朱戰(zhàn)霞, 賈國華, 等. 多介質(zhì)下空間目標的視覺測量[J]. 計算機應用, 2011, 31(5): 1431-1434. Wang Jun, Zhu Zhan-xia, Jia Guo-hua, et al. Multimedium space target visual measurement[J]. Journal of Computer Applications, 2011, 31(5): 1431-1434.

[5] 蔡迎波, 李德彪. 基于單目視覺的AUV水下定位方法[J]. 中國慣性技術(shù)學報, 2015, 23(4): 489-492. Cai Ying-bo, Li De-biao. AUV underwater positioning method based on monocular-vision[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 489-492.

[6] 王小剛, 郭繼峰, 崔乃剛. INS/Vision相對導航系統(tǒng)在無人機上的應用[J]. 哈爾濱工業(yè)大學學報, 2010, 42(7) 1029-1032 Wang Xiao-gang, Guo Ji-feng, Cui Nai-gang. A relative navigation method based on INS/Vision for UAV[J]. Journal of Harbin Institute of Technology, 2010, 42(7): 1029-1032.

[7] Park D B, Shin D H, Oh S H, et al. Development of a GPS/INS system for precision GPS guided bombs[J]. IEEE Aerospace & Electronic Systems Magazine, 2012, 27(3): 31-39.

[8] Moussa A, Ali A, El-Sheimy N, et al. The effect of time synchronization on real time implementation of integrated GPS/INS systems[C]//Proceedings of the International Technical Meeting of the Institute of Navigation, 2010: 43-48.

[9] Li Q, Wang L, Zhai C, et al. Time synchronization design based on FPGA in integrated GPS/INS system[C]//IEEE International Conference on Mechatronics and Automation. 2009: 3769-3774.

[10] Liu Kai-zhou, Li Jing, Guo Wei, et al. Navigation system of a class of underwater vehicle based on adaptive unscented Kalman fiter algorithm[J]. Journal of Central South University, 2014, 21(2): 550-557.

[11] He Hong-yang, Xu Jiang-ning, Li Jing-shu, et al. Improved fast backtracking alignment approach for strapdown inertial navigation system[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 179-183.

[12] Geng Y, Wang J. Adaptive estimation of multiple fading factors in Kalman filter for navigation applications[J]. GPS Solutions, 2008, 12(4): 273-279.

[13] Narasimhappa M, Rangababu P, Sabat S L, et al. A modified Sage-Husa adaptive Kalman filter for denoising fiber optic gyroscope signal[C]//2012 Annual IEEE India Conference. 2012: 1266-1271.

Adaptive integrated navigation method of visual positioning/INS in complex multi-medium environment

ZHANG Zhao-fei1,2, LUO Jian-jun1,2, GONG Bai-chun1,2, ZHU Zhan-xia1,2

(1. School of Astronautics, Northwestern Polytechnical University, Xi’an 710072, China; 2. Science and Technology on Aerospace Flight Dynamics Laboratory, Xi’an 710072, China)

In view that the autonomous underwater vehicle in the grounded microgravity environment of hybrid suspension has accurate real-time navigation problem in spacecraft rendezvous and docking, an adaptive integrated navigation scheme and algorithm was presented based on visual and inertial measurement system under multi-medium environment. The time synchronization between the data of the inertial measurement unit and that of the visual positioning system (VPS) was realized by fitting the inertial measurement data based on the time tag of VPS and backward inertial navigation calculation. Then an adaptive Sage-Husa integrated Kalman filter was designed based on exponent fading factor system with a real-time estimator for the measurement noise to solve the problem of time-varying measurement noise of VPS. The validity of the proposed algorithm was tested and verified by the experiments, and the results show that, compared with the integrated algorithm with common used Kalman filter and without time synchronization, the accurate real-time navigation can be achieved and the velocity/position estimation errors are reduced by 56% and 64% respectively.

visual positioning system; inertial navigation system; synchronization; adaptive Kalman filter; integrated navigation

A

1005-6734(2016)02-0190-06

10.13695/j.cnki.12-1222/o3.2016.02.010

2016-12-07

2016-03-23

國家自然科學基金（61004124）；中央高校基本科研業(yè)務費專項資金資助（3102014KYJD007）

張朝飛（1987—），男，博士研究生，從事慣性導航及組合導航研究。E-mail: fly060fly@qq.com

聯(lián) 系 人：羅建軍（1965—），男，教授，博士生導師。E-mail: jjluo@nwpu.edu.cn