張紅良，郭鵬宇，李 壯,

（1. 國防科學(xué)技術(shù)大學(xué) 航天科學(xué)與工程學(xué)院，長沙 410073；2. 中國人民解放軍63620部隊(duì)，甘肅 酒泉 732750）

一種視覺輔助的慣性導(dǎo)航系統(tǒng)動(dòng)基座初始對準(zhǔn)方法

張紅良1，郭鵬宇1，李 壯1,2

（1. 國防科學(xué)技術(shù)大學(xué) 航天科學(xué)與工程學(xué)院，長沙 410073；2. 中國人民解放軍63620部隊(duì)，甘肅 酒泉 732750）

針對慣性導(dǎo)航系統(tǒng)動(dòng)基座初始對準(zhǔn)問題，提出了一種視覺輔助的慣導(dǎo)系統(tǒng)動(dòng)基座初始對準(zhǔn)方法。建立了視覺與慣導(dǎo)系統(tǒng)測量模型，考慮了特征點(diǎn)位置已知和未知兩種情形，分別推導(dǎo)了視覺和慣導(dǎo)系統(tǒng)姿態(tài)位置間的關(guān)系，設(shè)計(jì)了EKF濾波器。建立了兩種情形下的濾波觀測方程，設(shè)計(jì)了晃動(dòng)基座初始對準(zhǔn)仿真實(shí)驗(yàn)，結(jié)果表明在視覺特征點(diǎn)位置已知和未知兩種條件下，濾波器狀態(tài)均能收斂，特征點(diǎn)位置已知時(shí)收斂時(shí)間小于30 s，特征點(diǎn)位置未知時(shí)收斂時(shí)間約為300 s；在陀螺零偏為0.01 (°)/h、加速度計(jì)零偏為50 μg的仿真條件下，對準(zhǔn)精度為水平姿態(tài)角優(yōu)于0.004°，方位角優(yōu)于0.06° 。提出的視覺輔助慣導(dǎo)系統(tǒng)動(dòng)基座初始對準(zhǔn)是一條較新且可行的思路。

慣性導(dǎo)航系統(tǒng)；初始對準(zhǔn)；動(dòng)基座；視覺

作為一種自主遞推式導(dǎo)航系統(tǒng)，慣性導(dǎo)航系統(tǒng)（INS）在導(dǎo)航前必須進(jìn)行初始對準(zhǔn)。初始對準(zhǔn)技術(shù)是慣性導(dǎo)航的關(guān)鍵技術(shù)之一[1-2]，初始對準(zhǔn)是確定慣性導(dǎo)航系統(tǒng)相對導(dǎo)航坐標(biāo)系初始姿態(tài)的過程，對準(zhǔn)的精度直接影響慣性導(dǎo)航精度。按照基座的運(yùn)動(dòng)狀態(tài)，初始對準(zhǔn)分為靜基座對準(zhǔn)和動(dòng)基座對準(zhǔn)兩種，其中動(dòng)基座對準(zhǔn)由于技術(shù)難度大，一直是初始對準(zhǔn)研究的熱點(diǎn)。傳遞對準(zhǔn)技術(shù)[3-4]是研究應(yīng)用較早的動(dòng)基座對準(zhǔn)技術(shù)，通過利用主慣導(dǎo)提供的姿態(tài)、速度、角速度等信息實(shí)現(xiàn)子慣導(dǎo)的動(dòng)基座對準(zhǔn)。近年來，一些研究者相繼提出了一些新的動(dòng)基座初始對準(zhǔn)方法，如以慣性坐標(biāo)系作為參考基準(zhǔn)進(jìn)行運(yùn)動(dòng)基座初始對準(zhǔn)的方法[2]；利用GPS、天文星體跟蹤器、里程計(jì)、大氣數(shù)據(jù)系統(tǒng)、多普勒計(jì)程儀輔助的動(dòng)基座對準(zhǔn)方法[5-7]，等等。由于攝像機(jī)成本低、體積小、自主性高，視覺慣性組合導(dǎo)航系統(tǒng)成為極具潛力的一種導(dǎo)航方式，成為近些年來組合導(dǎo)航系統(tǒng)領(lǐng)域研究的熱點(diǎn)[8-10]。本文利用攝像機(jī)測量位置和姿態(tài)作為觀測，提出了一種視覺輔助的慣性導(dǎo)航系統(tǒng)動(dòng)基座初始對準(zhǔn)方法，考慮了特征點(diǎn)位置已知和未知兩種情形，設(shè)計(jì)了EKF濾波器，進(jìn)行了晃動(dòng)基座對準(zhǔn)仿真實(shí)驗(yàn)，結(jié)果表明在特征點(diǎn)位置已知和未知兩種條件下，濾波器狀態(tài)收斂，視覺輔助慣導(dǎo)對準(zhǔn)能夠達(dá)到較高精度。本文利用視覺測量輔助慣性導(dǎo)航系統(tǒng)實(shí)現(xiàn)初始對準(zhǔn)是一條較新且可行的思路。

1 視覺與慣導(dǎo)系統(tǒng)測量模型

將攝像機(jī)與慣導(dǎo)系統(tǒng)固定安裝在一起（如圖1所示），以 INS的慣性器件組合（IMU）的敏感中心為原點(diǎn)，定義慣導(dǎo)系統(tǒng)體坐標(biāo)系（b系）為 Ob- XbYbZb，同時(shí)定義視覺測量坐標(biāo)系（c系）為 Oc- XcYcZc：攝像機(jī)光心為原點(diǎn)，光軸為 Zc軸，XcYc平面與成像靶面平行。由于固定安裝，慣導(dǎo)體坐標(biāo)系（b系）與視覺測量坐標(biāo)系（c系）間的相對姿態(tài)和位置關(guān)系、保持不變。

定義導(dǎo)航坐標(biāo)系（n系）為 On- XnYnZn，慣導(dǎo)系統(tǒng)對準(zhǔn)即是確定慣導(dǎo)系統(tǒng)體坐標(biāo)系到導(dǎo)航坐標(biāo)系間的姿態(tài)轉(zhuǎn)換關(guān)系。對于動(dòng)基座對準(zhǔn)的慣導(dǎo)系統(tǒng)，如圖2所示b系相對n系處于運(yùn)動(dòng)狀態(tài)，設(shè)t時(shí)刻，慣導(dǎo)系統(tǒng)的位置和姿態(tài)矩陣分別為 Pn(t)、(t)，t1到t2時(shí)刻慣導(dǎo)體坐標(biāo)系姿態(tài)變化為位置變化為 ΔPn= Pn(t2) -Pn(t1)。

根據(jù)視覺測量基本原理，利用攝像機(jī)對視場場景連續(xù)成像，提取跟蹤圖像中的特征點(diǎn)，根據(jù)特征點(diǎn)的像點(diǎn)變化可以估計(jì)攝像機(jī)的運(yùn)動(dòng)。我們考慮如下兩種情形的測量情形。

1）特征點(diǎn)位置已知情形

若特征點(diǎn)為人為設(shè)置的控制點(diǎn)，其在導(dǎo)航坐標(biāo)系中的位置已知，則根據(jù)視覺成像原理，特征點(diǎn)i的像點(diǎn)坐標(biāo)滿足共線方程：

圖1 視覺與慣導(dǎo)系統(tǒng)坐標(biāo)系Fig.1 Coordinate systems of vision and INS

圖2 視覺與慣導(dǎo)系統(tǒng)測量示意圖Fig.2 Vision and INS measurement description

若t時(shí)刻有多個(gè)特征點(diǎn)在攝像機(jī)中成像，則根據(jù)(1)式確定的共線方程可以求解該時(shí)刻攝像機(jī)的姿態(tài)(t)和位置(t )，它們與慣導(dǎo)系統(tǒng)姿態(tài)(t)和位置 Pn(t)的關(guān)系為：

2）特征點(diǎn)位置未知情形

通過跟蹤相鄰圖像幀共同特征點(diǎn)，采用單目視覺運(yùn)動(dòng)估計(jì)方法（如視覺里程計(jì)方法），可以得到t1、t2時(shí)刻間攝像機(jī)的姿態(tài)變化和位置變化單位矢量，它們與慣導(dǎo)系統(tǒng)位置姿態(tài)之間的關(guān)系為

2 視覺輔助慣導(dǎo)對準(zhǔn)濾波器模型

設(shè)計(jì)EKF濾波器，根據(jù)視覺系統(tǒng)測量估計(jì)慣導(dǎo)系統(tǒng)姿態(tài)誤差，實(shí)現(xiàn)慣導(dǎo)系統(tǒng)初始對準(zhǔn)。定義濾波器狀態(tài)為

將公式(7)(8)線性化，可將濾波器的狀態(tài)方程寫成如式(9)所示形式：

根據(jù)視覺與慣導(dǎo)系統(tǒng)測量模型，當(dāng)特征點(diǎn)位置已知時(shí)，由公式(2)(3)，濾波器觀測為：

式中，M at2Ang｛·｝表示姿態(tài)轉(zhuǎn)換矩陣對應(yīng)的姿態(tài)角，Vc為視覺姿態(tài)位置測量誤差和高階誤差項(xiàng)引起的觀測噪聲。

當(dāng)特征點(diǎn)位置未知時(shí)，由公式(4)(5)，t2時(shí)刻濾波器觀測為式(11)所示。式(11)中為視覺測量誤差和高階誤差項(xiàng)引起的觀測噪聲。

根據(jù)公式(10)或(11)，對狀態(tài)線性化，可將濾波器的觀測方程寫成如下形式：

3 仿 真

仿真假設(shè)對某晃動(dòng)平臺(tái)上的慣導(dǎo)系統(tǒng)進(jìn)行初始對準(zhǔn)，如圖3所示，動(dòng)平臺(tái)坐標(biāo)系原點(diǎn)到慣導(dǎo)系統(tǒng)體坐標(biāo)系原點(diǎn)的矢量為=[5 2 0.5]Tm，對準(zhǔn)中動(dòng)平臺(tái)繞坐標(biāo)系原點(diǎn) Op搖擺，其中繞 Xp軸的搖擺幅度為± 15°，搖擺周期為 5 s，繞 Yp軸的搖擺幅度為±1°，搖擺周期為20 s，繞 Zp軸的搖擺幅度為±5°，搖擺周期為10 s。搖擺運(yùn)動(dòng)導(dǎo)致的慣導(dǎo)系統(tǒng)位置和姿態(tài)角的變化如圖4所示。

圖3 視覺輔助慣導(dǎo)系統(tǒng)對準(zhǔn)仿真Fig.3 Vision aided INS alignment simulation

圖4 仿真中慣導(dǎo)的位置變化（上圖）和姿態(tài)角變化（下圖）Fig.4 INS position(upside) and attitude(downside) in simulation

圖5 特征點(diǎn)位置已知情形濾波器狀態(tài)估計(jì)誤差Fig.5 Errors of filter states estimation in case knowing the vision character points positions

濾波器初值誤差設(shè)置為：慣導(dǎo)水平姿態(tài)誤差5°，方位角誤差10°，慣導(dǎo)位置誤差5 m，速度誤差1 m/s，視覺與慣導(dǎo)坐標(biāo)系間的姿態(tài)誤差 5°，視覺測量坐標(biāo)系原點(diǎn)在慣導(dǎo)坐標(biāo)系中的位置誤差0.5 m。

1）特征點(diǎn)位置已知情形

仿真假設(shè)攝像機(jī)成像頻率為 20 Hz，攝像機(jī)拍攝位置已知的特征點(diǎn)，通過提取圖像中的這些特征點(diǎn)計(jì)算出視覺系統(tǒng)的位置和姿態(tài)，其中姿態(tài)計(jì)算精度設(shè)置為 0.15°，位置計(jì)算精度設(shè)置為0.2 m。采用公式(10)所示的觀測模型，一次仿真視覺輔助慣導(dǎo)對準(zhǔn)濾波器的估計(jì)誤差如圖5所示。

由圖5可以看出，慣導(dǎo)速度、姿態(tài)、位置和視覺慣導(dǎo)間的相對姿態(tài)估計(jì)誤差都能在較短的時(shí)間內(nèi)收斂到零附近；視覺慣導(dǎo)相對位置也明顯收斂，但收斂速度相對慢些。該次仿真中，對準(zhǔn)100 s后慣導(dǎo)的水平姿態(tài)角對準(zhǔn)精度優(yōu)于0.004°，方位角對準(zhǔn)精度優(yōu)于0.06°。仿真表明在采用已知特征點(diǎn)的條件下，視覺輔助慣導(dǎo)對準(zhǔn)是可行的，且收斂速度快，收斂精度高。

2）特征點(diǎn)位置未知情形

該情形下，利用視覺系統(tǒng)跟蹤測量圖像中共同的特征點(diǎn)，根據(jù)相鄰兩幀共同特征點(diǎn)的像點(diǎn)關(guān)系，利用視覺導(dǎo)航方法求解相鄰兩幀間攝像機(jī)的姿態(tài)變化和位置變化量的單位矢量，假設(shè)姿態(tài)變化計(jì)算精度為 0.15°，位置變化單位矢量方向計(jì)算精度為 0.15°，攝像機(jī)成像頻率為20 Hz。根據(jù)文獻(xiàn)[9]的相關(guān)結(jié)論，在僅觀測姿態(tài)和位置變化量的條件下，慣導(dǎo)位置是不可觀測的，因此仿真中不考慮慣導(dǎo)位置狀態(tài)。利用公式(11)所示的觀測模型，一次仿真視覺輔助慣導(dǎo)對準(zhǔn)濾波器的估計(jì)誤差如圖6所示。

從圖6可以看出，慣導(dǎo)速度、姿態(tài)及視覺與慣導(dǎo)間的相對位姿均收斂，估計(jì)誤差收斂到零附近。其中慣導(dǎo)速度、水平姿態(tài)和視覺慣導(dǎo)間的相對姿態(tài)收斂速度較快，受基座晃動(dòng)的影響，慣導(dǎo)速度估計(jì)表現(xiàn)出一定的振蕩趨勢。慣導(dǎo)方位角和視覺慣導(dǎo)間的相對位置收斂時(shí)間相對較長，但300 s左右已基本收斂到真值附近。該次仿真中，對準(zhǔn)400 s后慣導(dǎo)的水平姿態(tài)角對準(zhǔn)精度優(yōu)于0.004°，方位角對準(zhǔn)精度優(yōu)于0.06° 。仿真表明在采用位置未知特征點(diǎn)的條件下，觀測攝像機(jī)的位置和姿態(tài)變化視覺輔助慣導(dǎo)對準(zhǔn)是可行的。

圖6 特征點(diǎn)位置未知情形濾波器狀態(tài)估計(jì)誤差Fig.6 Errors of filter states estimation in case not knowing the vision character points positions

4 結(jié) 論

本文研究了一種視覺輔助的慣性導(dǎo)航系統(tǒng)動(dòng)基座初始對準(zhǔn)方法，利用仿真實(shí)驗(yàn)證明了方法的可行性。文章建立了視覺與慣導(dǎo)系統(tǒng)測量模型，設(shè)計(jì)了EKF濾波器，包含慣導(dǎo)速度、姿態(tài)、位置和攝像機(jī)與慣導(dǎo)間的相對姿態(tài)和位置等狀態(tài)，考慮了特征點(diǎn)位置已知和未知兩種情形，推導(dǎo)了不同的濾波器觀測模型。設(shè)計(jì)了晃動(dòng)基座對準(zhǔn)仿真實(shí)驗(yàn)，考慮了慣性器件測量誤差和視覺測量誤差，結(jié)果表明在特征點(diǎn)位置已知和未知兩種條件下，濾波器狀態(tài)均收斂，在特征點(diǎn)位置已知情形下，收斂時(shí)間小于30 s，特征點(diǎn)位置未知情形下，收斂時(shí)間約為300 s。狀態(tài)收斂后，視覺輔助慣導(dǎo)對準(zhǔn)精度水平姿態(tài)角優(yōu)于 0.004°，方位角優(yōu)于0.06°。本文研究的視覺測量輔助慣性導(dǎo)航系統(tǒng)實(shí)現(xiàn)初始對準(zhǔn)是一條較新且可行的思路。

（References）：

[1] 萬德鈞，房建成. 慣性導(dǎo)航初始對準(zhǔn)[M]. 南京：東南大學(xué)出版社，1998.

[2] 練軍想. 捷聯(lián)慣導(dǎo)動(dòng)基座對準(zhǔn)新方法及導(dǎo)航誤差抑制技術(shù)研究[D]. 長沙：國防科學(xué)技術(shù)大學(xué), 2007.

LIAN Jun-xiang. Research on a new moving-base alignment approach and error depression of strapdown inertial navigation system[D]. Changsha: National University of Defense Technology, 2007.

[3] Baziw J, Leondes C T. In-flight alignment and calibration of inertial measurement units-Part 1: General formulation[J]. IEEE Transactions on Aerospace and Electronic Systems, 1972, AES-8(4): 439-449.

[4] Baziw J, Leondes C T. In-flight alignment and calibration of inertial measurement units-Part II: Experimental results[J]. IEEE Transactions on Aerospace and Electronic Systems, 1972, AES-8(4): 450-465.

[5] 馬建萍. GPS輔助捷聯(lián)慣導(dǎo)系統(tǒng)動(dòng)基座初始對準(zhǔn)新方法[J]. 傳感技術(shù)學(xué)報(bào), 2010, 23(11): 1656-1661.

MA Jian-ping. A new GPS aided initial alignment algorithm for SINS on moving base[J]. Chinese Journal of Sensors and Actoators, 2010, 23(11): 1656-1661.

[6] 張金亮，秦永元，吳楓. 捷聯(lián)慣導(dǎo)基于星體跟蹤器的高精度初始對準(zhǔn)算法[J]. 中國慣性技術(shù)學(xué)報(bào)，2013，21(1)：22-25, 36.

ZHANG Jin-liang, QIN Yong-yuan, WU Feng. Precise SINS initial alignment algorithm based on star tracker[J]. Journal of Chinese Inertial Technology, 2013, 21(1): 22-25, 36.

[7] 楊波，彭培林，王躍鋼. 里程計(jì)輔助捷聯(lián)慣導(dǎo)運(yùn)動(dòng)基座對準(zhǔn)方法[J]. 中國慣性技術(shù)學(xué)報(bào)，2013，21(3)：298-301, 307.

YANG Bo, PENG Pei-lin, WANG Yue-gang, ZHOU Xiao-gang. Alignment method of strapdown inertial navigation system aided by odometer on moving base[J]. Journal of Chinese Inertial Technology, 2013, 21(3): 298-301, 307.

[8] Mirzaei F M, Roumeliotis S I. A Kalman filter-based algorithm for IMU-camera calibration: Observability analysis and performance evaluation[J]. IEEE Transactions on Robotics, 2008, 24(5): 1143-1156.

[9] Li M, Mourikis A I. High-precision, consistent EKF-based visualinertial odometry[J]. The International Journal of Robotics Research, 2013, 32(6): 690-711.

[10] Fang Q, Huang X. UKF for integrated vision and inertial navigation based on three-view geometry[J]. IEEE Sensors Journal, 2013, 13(7): 2711-2719.

Vision aided alignment method for inertial navigation system on moving base

ZHANG Hong-liang1, GUO Peng-yu1, LI Zhuang1,2
(1. College of Aerospace Science and Engineering, National University of Defense Technology, Changsha 410073, China; 2. Unit 63620 of PLA, Jiuquan 732750, China)

Inertial navigation system(INS) alignment on moving base is a difficult task. This paper proposes a vision aided alignment method for INS on moving base. The measurement model of vision aided INS is established. Considering the two cases that the positions of the vision character points are known or not, the relationships between the attitude and position measurements for the vision system and the INS are deduced respectively An extended kalman filter(EKF) is designed for the alignment, and the EKF observations for both cases are given separately. Alignment simulations on swinging base were performed. Results show that in both cases the states converge and the convergence times are less than 30 s and about 300 s respectively. In the simulations the INS is supposed to be consisted of gyroscopes with bias 0.01 (°)/h and accelerometers with bias 50 μg, and the INS alignment acquire results with precision of 0.004° for the level attitude and 0.06° for the azimuth. The proposed method gives a new and feasible way for the INS alignment on moving base.

inertial navigation system; alignment; moving base; vision

張紅良(1981—)，男，講師，從事組合導(dǎo)航技術(shù)研究。E-mail：mrzhanghongliang@163.com

1005-6734(2014)04-0469-05

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

U666.1

A

2014-03-08；

2014-07-11

國家自然科學(xué)基金（11072263)）