一種復雜多介質環境下的視覺/慣性自適應組合導航方法

張朝飛，羅建軍，龔柏春，朱戰霞

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

一種復雜多介質環境下的視覺/慣性自適應組合導航方法

張朝飛1,2，羅建軍1,2，龔柏春1,2，朱戰霞1,2

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

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

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

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

1 多介質環境下視覺/慣性組合導航系統的組成及原理

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

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

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

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

圖2 視覺定位系統原理圖Fig.2 Schematic of visual positioning system

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

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

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

圖3 組合導航系統流程圖Fig.3 Flow chart of integrated navigation system

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

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

首先INS進行導航解算。當CCD相機拍攝AUV圖片時，上位機自動記錄 CCD相機的拍攝時刻，根據式(2)計算對應的增量信息。INS進行周期自適應的導航解算，實時輸出AUV的姿態、速度和位置信息。當VPS信息更新時，上位機自動記錄其更新時刻。然后依據存儲的陀螺、加速度計和 CCD相機拍攝時刻所對應到IMU采樣周期內的時刻t進行周期自適應的逆向導航，利用逆向導航完成時的姿態、速度取反和位置信息與 VPS的位置信息進行組合導航，對 INS的誤差進行修正。然后根據存儲的正向陀螺和加速度計采樣數據進行周期自適應的INS導航解算，實時輸出VPS信息更新時刻之后AUV相對于當地地理坐標系下的姿態、速度和位置信息。

逆向導航的過程如下：陀螺和加速度計采樣數據進行存儲后按時間進行正向和逆向排列，形成正向序列和逆向序列。由正向導航的解算公式推導出逆向導航的解算公式。假設正向導航的周期為n個，以第n個周期的姿態、速度取反和以位置作為逆向導航的初始姿態、初始速度和初始位置，將陀螺采樣和地球自轉角速率取反，并且對陀螺和加速度計的采樣數據進行逆序處理，形成逆向數據序列。若不考慮計算誤差，逆向導航時各時刻的姿態信息、位置信息與正向導航在對應時刻上的姿態信息、位置信息相同，速度信息大小相同，方向相反。由上述推導可知，正向導航和逆向導航的解算式相同，因此其系統誤差方程也相同，逆向導航的方程與正向導航的方程完全相同，僅導航過程中的輸出信息不同[11]。

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

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

視覺/慣性組合導航系統的狀態方程用連續微分方程描述，其狀態方程和量測方程分別為

根據線性系統理論對式(3)進行離散化，離散化后的狀態方程為

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

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

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

4 實驗驗證

利用空間交會對接地面混合懸浮微重力模擬實驗系統完成AUV和目標體的交會對接實驗，調整浮力系統使 AUV處于懸浮狀態模擬微重力環境。控制系統通過通信光纖完成 IMU數據和推進器的控制指令的傳輸，雙目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中標定過的軌跡進行運動，實驗過程中的計算條件和技術參數見表1。

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

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

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

5 總 結

1）本文提出的多介質環境下視覺/慣性的自適應組合導航的方法，通過INS擬合CCD相機拍攝時刻的增量輸出，進行逆向導航后實現與 VPS的時間同步，減小了由于時間不同步造成的導航誤差，避免了時間不同步造成的組合導航發散問題.

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

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

（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] 王俊, 朱戰霞, 賈國華, 等. 多介質下空間目標的視覺測量[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]. 中國慣性技術學報, 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相對導航系統在無人機上的應用[J]. 哈爾濱工業大學學報, 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）；中央高校基本科研業務費專項資金資助（3102014KYJD007）

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

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