趙慧, 熊智, 施麗娟, 郁豐, 林愛軍
(1.南京航空航天大學(xué) 自動化學(xué)院,江蘇 南京 210016; 2.南京航空航天大學(xué) 航天學(xué)院,江蘇 南京 210016)
基于慣性系下陀螺誤差在線估計修正的慣性與星光組合導(dǎo)航方法
趙慧1, 熊智1, 施麗娟1, 郁豐2, 林愛軍1
(1.南京航空航天大學(xué) 自動化學(xué)院,江蘇 南京 210016; 2.南京航空航天大學(xué) 航天學(xué)院,江蘇 南京 210016)
傳統(tǒng)慣性與星光組合通常需要將慣性系下的星光姿態(tài)信息轉(zhuǎn)換到導(dǎo)航坐標系進而與慣性導(dǎo)航系統(tǒng)進行姿態(tài)組合,由于姿態(tài)信息轉(zhuǎn)換過程中通常需要引入地理位置信息實現(xiàn)轉(zhuǎn)換,從而不可避免地引入轉(zhuǎn)換誤差,無法充分發(fā)揮高精度星光姿態(tài)信息對慣性導(dǎo)航誤差的修正作用??紤]到陀螺原始輸出信息和星光姿態(tài)信息均能直接在慣性參考坐標系下測量獲得,設(shè)計了一種基于慣性系下陀螺誤差在線估計修正的慣性與星光組合導(dǎo)航方案。通過建立基于慣性系下陀螺誤差估計修正的慣性與星光組合導(dǎo)航數(shù)學(xué)模型,直接在慣性系下對陀螺漂移誤差進行在線開環(huán)跟蹤估計;通過對陀螺誤差實時修正,能夠有效減小由于陀螺漂移所帶來的慣性導(dǎo)航系統(tǒng)解算誤差。仿真結(jié)果表明,該方案能夠有效估計出陀螺的漂移誤差,進而有效提高了慣性導(dǎo)航系統(tǒng)精度。
控制科學(xué)與技術(shù); 慣性與星光組合; 陀螺漂移誤差; 慣性坐標系; 開環(huán)估計修正
星敏感器作為一種高精度的姿態(tài)測量儀器,可直接提供載體相對于慣性坐標系的角秒級姿態(tài)信息[1-2],具有獨立、自主工作的能力,在航空、航天等領(lǐng)域得到了較為廣泛的應(yīng)用[3-5]??紤]到星敏感器提供的姿態(tài)信息具有誤差不隨導(dǎo)航時間發(fā)散的特點,其與慣性導(dǎo)航系統(tǒng)二者具有較強的互補性[6],因而慣性與星光組合導(dǎo)航得到了廣泛關(guān)注,較多學(xué)者亦針對慣性與星光組合導(dǎo)航開展了研究工作。
文獻[7]在研究過程中將星敏感器輸出的相對于慣性坐標系的姿態(tài)信息通過轉(zhuǎn)換得到地理坐標系下的姿態(tài)信息,進而與慣性導(dǎo)航系統(tǒng)進行組合,該轉(zhuǎn)換過程耦合了由慣性器件漂移所帶來的導(dǎo)航誤差,導(dǎo)致無法充分發(fā)揮高精度星敏感器姿態(tài)信息的修正作用;文獻[8]在研究過程中則將慣性導(dǎo)航系統(tǒng)輸出的相對于地理坐標系的姿態(tài)信息轉(zhuǎn)換到慣性坐標系下,該轉(zhuǎn)換過程同樣耦合了慣性導(dǎo)航位置誤差;文獻[9]針對上述耦合誤差模型進行了研究,提出了耦合位置誤差模型的慣性與星光組合算法。這些方法均需要完成姿態(tài)的轉(zhuǎn)換計算過程,會不可避免引入轉(zhuǎn)換誤差,無法充分發(fā)揮星光高精度姿態(tài)的誤差修正作用。
考慮到陀螺漂移誤差是影響慣性導(dǎo)航系統(tǒng)性能的主要誤差源之一,為了有效利用高精度星光姿態(tài)信息實現(xiàn)對慣性誤差的直接修正,提高慣性與星光組合導(dǎo)航性能,本文提出了基于慣性系下陀螺誤差在線估計修正的慣性與星光組合導(dǎo)航方法,通過建立慣性系下系統(tǒng)狀態(tài)方程和量測方程,實現(xiàn)對陀螺漂移誤差的估計校正,有效提高了慣性與星光組合導(dǎo)航性能,為慣性與星光組合導(dǎo)航的工程化應(yīng)用提供了參考。
基于數(shù)據(jù)融合技術(shù)的組合導(dǎo)航需要將不同導(dǎo)航系統(tǒng)輸出的信息在相同參考坐標系下進行融合,然而,以地理系為參考系的慣性導(dǎo)航系統(tǒng)輸出的導(dǎo)航信息和星光信息參考坐標系不一致,無法直接進行組合,需要將星敏感器輸出的相對于慣性坐標系的姿態(tài)信息轉(zhuǎn)換到地理坐標系下,其轉(zhuǎn)換方式為
(1)
(2)
式中:λ為經(jīng)度;L為緯度。
圖1 基于慣性系下陀螺誤差估計修正的慣性與星光組合導(dǎo)航總體框圖Fig.1 Overall scheme of SINS/STAR integrated navigation system based on online estimation of gyro error in inertial coordinate
2.1 基于陀螺原始輸出的慣性系姿態(tài)計算模型
(3)
采用畢卡逼近法求解,并用等效旋轉(zhuǎn)矢量進行補償,可得四元數(shù)解析表達式[11-12]為
(4)
式中:Qi(t+T)為t+T時刻載體的姿態(tài)四元數(shù),T為采樣周期;I為4×4的單位矩陣;Δθ0、Δθ、[Δθ]分別對應(yīng)為
(5)
(6)
(7)
此外,由于計算誤差等因素的存在,使得計算過程中的四元數(shù)范數(shù)不再等于1,即四元數(shù)會逐漸失去規(guī)范性,而表征旋轉(zhuǎn)的四元數(shù)應(yīng)該為規(guī)范化四元數(shù),因而還需對遞推得到的四元數(shù)進行周期性規(guī)范化處理,即將計算得到的四元數(shù)中的每一項除以該四元數(shù)的范數(shù),從而得到規(guī)范化的四元數(shù)[13-14]。
2.2 陀螺誤差在線估計模型
在由陀螺儀輸出獲得載體相對于慣性坐標系的姿態(tài)信息Qi后,進一步結(jié)合星敏感器姿態(tài)量測信息Qc,通過建立慣性坐標系下系統(tǒng)的狀態(tài)方程和量測方程,經(jīng)由Kalman濾波器,即可估計和跟蹤陀螺儀的漂移誤差,通過實時修正陀螺漂移誤差,從而提高地理系下慣性導(dǎo)航系統(tǒng)性能。有關(guān)陀螺誤差在線估計模型如下。
2.2.1 陀螺誤差在線估計狀態(tài)方程推導(dǎo)
1)誤差狀態(tài)建模
記載體相對于地心慣性坐標系的真實姿態(tài)四元數(shù)為Q,則
(8)
(8)式兩邊對時間求導(dǎo),可得
(9)
根據(jù)四元數(shù)運動學(xué)微分方程,可得載體真實姿態(tài)四元數(shù)微分方程為
(10)
對應(yīng)地估計四元數(shù)的微分方程為
(11)
(12)
將(8)式、(10)式、(11)式代入(9)式,根據(jù)四元數(shù)運算規(guī)則,化簡可得
(13)
進一步地根據(jù)四元數(shù)運算規(guī)則,將(13)式展開可得
(14)
與此同時,在姿態(tài)誤差為小量的情況下,誤差四元數(shù)δQ[15]可近似為
(15)
將(15)式代入(14)式,忽略2階小量,即可得
(16)
(17)
式中:隨機游走εb的誤差模型為
(18)
(19)
同理,載體估計角速率可表示為
(20)
因而,
(21)
將(21)式代入(16)式可得
(22)
從(22)式的表達形式中可看出,誤差四元數(shù)δQ矢量部分δq13的微分方程反映了陀螺漂移誤差,為此,本文選擇誤差四元數(shù)的矢量部分δq13以及陀螺隨機游走誤差δεb作為系統(tǒng)狀態(tài)量,建立系統(tǒng)的狀態(tài)方程。
2)組合誤差狀態(tài)模型
系統(tǒng)狀態(tài)變量定義為
(23)
結(jié)合(20)式以及(16)式,可得
F(t)X(t)+G(t)W(t),
(24)
式中:F(t)為系統(tǒng)狀態(tài)系數(shù)矩陣;G(t)為系統(tǒng)噪聲矩陣;W(t)為系統(tǒng)噪聲。
2.2.2 慣性與星光組合導(dǎo)航系統(tǒng)量測方程推導(dǎo)
在建立系統(tǒng)狀態(tài)方程的基礎(chǔ)上,進一步地需要推導(dǎo)建立系統(tǒng)量測方程。
當(dāng)星敏感器坐標系s系與載體坐標系b系重合時,即不考慮星敏感器的安裝誤差的條件下,其輸出的姿態(tài)信息Qc即為載體相對于慣性坐標系的姿態(tài)。由于星敏感器觀測存在一定的測量誤差,因而姿態(tài)信息Qc并非為真實載體相對于慣性坐標系的姿態(tài)四元數(shù),其包含了誤差信息,即
Qc=Q?δQε,
(25)
當(dāng)誤差為小量時,誤差四元數(shù)可近似為
(26)
將(24)式代入(23)式,展開后化簡可得
(27)
式中:
(28)
Qi=Q?δQ-1,
(29)
式中:δQ-1為誤差四元數(shù)δQ的逆。
根據(jù)陀螺儀輸出求解得到姿態(tài)四元數(shù)Qi和天文定姿解算得到的姿態(tài)四元數(shù)Qc,構(gòu)建系統(tǒng)量測方程為
Zs(t)=(Qi)-1?Qc=
(Q?δQ-1)-1?(Q+δQε1)=
δQ+(Qi)-1?δQε1,
(30)
(31)
3.1 仿真基本條件設(shè)置
為了驗證本文所提出算法的有效性,本節(jié)首先測試慣性系下慣性與星光組合對陀螺漂移誤差的估計情況,繼而比較經(jīng)過陀螺誤差修正前后的慣性導(dǎo)航系統(tǒng)誤差??紤]到星敏感器的適用范圍,本文主要結(jié)合空天飛行器在軌飛行特性進行研究,選取飛行高度為500 km、飛行速度為7.11 km/s的近地軌道來近似模擬空天飛行器飛行航跡,其在慣性系下的航跡如圖2所示,傳感器仿真參數(shù)設(shè)置如表1所示。為有效分析陀螺漂移誤差對導(dǎo)航系統(tǒng)的影響,本文在仿真過程中暫不考慮加速度計誤差。仿真時長共計120 min.
圖2 慣性系下的運動航跡Fig.2 Path of motion in inertial coordinate
3.2 基于慣性系下陀螺誤差估計修正的慣性與星光組合導(dǎo)航仿真驗證
圖3中,圖3(a)為陀螺儀實際誤差和估計誤差對比曲線,圖3(b)為減去估計值后陀螺的殘余誤差。從圖3仿真結(jié)果看,采用基于慣性系下慣性與星光組合方法估計出的陀螺隨機游走誤差基本上能跟蹤上實際陀螺隨機游走誤差,其誤差的殘余誤差量級小于0.5°/h,且殘余誤差特性較接近白噪聲。從表2陀螺漂移誤差統(tǒng)計表看,估計的陀螺隨機游走誤差統(tǒng)計特性和實際陀螺隨機游走誤差量級一致,數(shù)值接近。由此可見,采用基于慣性系下的慣性與天文姿態(tài)組合方法可較為有效地估計出陀螺隨機游走誤差。
表1 傳感器參數(shù)設(shè)置Tab.1 Sensor parameters
圖3 陀螺誤差估計曲線Fig.3 Estimated gyro error curves
表2 陀螺誤差統(tǒng)計Fig.3 Statistics of gyro errors
為了進一步測試陀螺誤差修正對慣性導(dǎo)航系統(tǒng)性能的影響,在估計得到陀螺誤差后,將其反饋給地理系下慣性導(dǎo)航系統(tǒng),進一步比較采用陀螺誤差修正的地理系慣性導(dǎo)航系統(tǒng)和不進行陀螺誤差修正的地理系慣性導(dǎo)航系統(tǒng)性能。并且,在相同條件下采用了慣性與星光組合導(dǎo)航進行了仿真測試和對比。圖4采用陀螺誤差估計值進行誤差修正的地理系慣性導(dǎo)航系統(tǒng)導(dǎo)航參數(shù)誤差曲線,圖5為不進行陀螺誤差修正的地理系慣性導(dǎo)航系統(tǒng)導(dǎo)航參數(shù)誤差曲線,圖6為傳統(tǒng)慣性與星光組合導(dǎo)航誤差曲線。
圖4 陀螺誤差修正下的地理系慣性導(dǎo)航誤差曲線Fig.4 Inertial navigation errors based on gyro error corrected in geographic coordinate
圖5 未修正情況下地理系慣性導(dǎo)航誤差曲線Fig.5 Inertial navigation errors based on gyro error uncorrected in geographic coordinate
圖6 傳統(tǒng)慣性與星光組合導(dǎo)航誤差曲線Fig.6 Navigation error curves of traditional SINS/STAR integrated navigation system
比較圖4(a)陀螺誤差修正下地理系慣性導(dǎo)航姿態(tài)誤差曲線和圖5(a)陀螺誤差未修正情況下地理系慣性導(dǎo)航姿態(tài)誤差曲線,發(fā)現(xiàn)陀螺誤差修正后的姿態(tài)誤差量級較未做陀螺誤差修正的慣性導(dǎo)航姿態(tài)解算誤差提高約一個數(shù)量級,由此可知,將由慣性系下慣性與星光組合估計出的陀螺誤差反饋補償?shù)降乩硐祽T性導(dǎo)航解算中,可以有效提高地理系慣性導(dǎo)航姿態(tài)解算精度。同樣,比較圖4(b)和圖5(b),圖4(c)和圖5(c),可發(fā)現(xiàn)陀螺誤差修正后的位置、速度誤差較未進行陀螺誤差修正情況下位置、速度誤差量級減小,導(dǎo)航性能明顯改善。從表3導(dǎo)航性能統(tǒng)計結(jié)果上亦可發(fā)現(xiàn),經(jīng)過陀螺誤差修正的導(dǎo)航系統(tǒng)誤差量級明顯小于未修正情況。此外,比較圖6(a)傳統(tǒng)慣性與星光組合導(dǎo)航的姿態(tài)誤差曲線和圖5(a)未修正情況下地理系慣性導(dǎo)航姿態(tài)誤差曲線,發(fā)現(xiàn)傳統(tǒng)慣性與星光組合導(dǎo)航的姿態(tài)誤差要明顯小于單一慣性導(dǎo)航系統(tǒng)姿態(tài)誤差。也就是說,傳統(tǒng)的慣性與星光組合可以較為有效地修正導(dǎo)航系統(tǒng)姿態(tài)誤差。進一步比較圖4(a)和圖6(a),可以發(fā)現(xiàn)傳統(tǒng)的慣性與星光組合導(dǎo)航姿態(tài)誤差要略大于陀螺誤差修正后的姿態(tài)誤差,且誤差曲線呈現(xiàn)發(fā)散的趨勢。由此說明,傳統(tǒng)慣性與星光組合雖然利用了星光信息,但由于在組合過程中需要對星光信息進行坐標轉(zhuǎn)換而耦合進其他誤差源,因而未能充分發(fā)揮高精度星光姿態(tài)信息對慣性導(dǎo)航誤差的修正作用。
綜上所述,本文所提出的基于慣性系下陀螺誤差估計修正的慣性與星光組合導(dǎo)航方法有效。
陀螺漂移誤差作為慣性導(dǎo)航系統(tǒng)的重要誤差源,嚴重影響慣性導(dǎo)航系統(tǒng)解算精度。傳統(tǒng)慣性與星光組合通過坐標轉(zhuǎn)換實現(xiàn)星光信息對慣性導(dǎo)航系統(tǒng)的姿態(tài)修正,然而由于轉(zhuǎn)換所帶來的誤差使得修正效果有限。針對這一問題,本文提出了基于慣性系下陀螺誤差估計修正的慣性與星光組合導(dǎo)航方法。首先基于陀螺儀原始輸出計算得到載體相對慣性系姿態(tài)信息,繼而結(jié)合星光信息推導(dǎo)建立了慣性坐標系下的慣性與星光組合導(dǎo)航系統(tǒng)模型,實現(xiàn)對陀螺漂移誤差的有效估計,最后通過開環(huán)校正對陀螺漂移誤差進行修正,在不影響慣性導(dǎo)航系統(tǒng)基本解算流程的基礎(chǔ)上,有效減小了由于陀螺漂移所帶來的慣性導(dǎo)航系統(tǒng)解算誤差。仿真結(jié)果表明,本文所提出的方法能夠有效估計出陀螺的漂移誤差,經(jīng)過修正導(dǎo)航系統(tǒng)性能明顯提高,本文所提出的方法為星敏感器的有效利用提供參考。
表3 慣性導(dǎo)航系統(tǒng)誤差均方根值統(tǒng)計Tab.3 RMS statistics of inertial navigation errors
References)
[1] Ma L H, Wang X S, Zhan D J, et al. Extraction method of the motion blurred star image for the star sensor under high dynamic conditions[C]∥IEEE International Conference on Signal Processing. Hangzhou: IEEE, 2014:836-840.
[2] Zhang S D, Sun H H, Wang Y J, et al. Design of high precision star image locating method used in star sensor technology[C]∥2010 International Conference on Computer, Mechatronics, Control and Electronic Engineering. Changchun: IEEE, 2010: 411-414.
[3] Pal M, Bhat M S. Star sensor based spacecraft angular rate estimation independent of attitude determination[C]∥ IEEE International Conference on Control Applications (CCA). Hyderabad: IEEE, 2013: 580-585.
[4] Rad A M. Compensating and analysis error in height channel of inertial navigation system using star sensors[C]∥ IEEE Aerospace Conference. Montana: IEEE, 2015.
[5] 王融, 熊智, 劉建業(yè), 等. 一種星敏感器安裝誤差標定模型仿真研究[J]. 系統(tǒng)仿真技術(shù), 2013, 9(4): 287-291. WANG Rong, XIONG Zhi, LIU Jian-ye, et al. Study on installation error calibration model simulation of star sensor[J]. System Simulation Technology, 2013, 9(4): 287-291. (in Chinese)
[6] 張金亮, 秦永元, 成研. 捷聯(lián)慣導(dǎo)與星跟蹤器組合導(dǎo)航算法研究[J]. 宇航學(xué)報, 2013, 34(8): 1078-1083. ZHANG Jin-liang, QIN Yong-yuan, CHENG Yan. A SINS/star tracker integrated navigation algorithm[J]. Journal of Astronautics, 2013, 34(8): 1078-1083. (in Chinese)
[7] 王鵬, 張迎春. 基于SINS/星敏感器的組合導(dǎo)航模式[J]. 東南大學(xué)學(xué)報:自然科學(xué)版, 2005, 35(S2): 84-89. WANG Peng, ZHANG Ying-chun. Integrated navigation mode based on the SINS/star sensor[J]. Journal of Southeast University:Natural Science Edition, 2005, 35(S2): 84-89. (in Chinese)
[8] 趙長山, 秦永元, 夏家和. SINS/星敏感器組合導(dǎo)航方案研究[J]. 西北工業(yè)大學(xué)學(xué)報, 2008, 26(5): 631-635. (in Chinese) ZHAO Chang-shan, QIN Yong-yuan, XIA Jia-he. Improving online autonomous calibration of SINS/star sensor integrated system[J]. Journal of Northwestern Polytechnical University, 2008, 26(5): 631-635. (in Chinese)
[9] 熊智, 陳海明, 郁豐. 耦合位置誤差的機載慣性/星光組合算法研究[J]. 宇航學(xué)報, 2010, 31(12): 2683-2690. XIONG Zhi, CHEN Hai-ming, YU Feng. Research of airborne INS/star integrated algorithm coupled with position error[J]. Journal of Astronautics, 2010, 31(12): 2683-2690. (in Chinese)
[10] Peng H, Zhi X, Wang R, et al. A new dynamic calibration method for IMU deterministic errors of the INS on the hypersonic cruise vehicles[J]. Aerospace Science and Technology, 2014, 32(1): 121-130.
[11] 熊智, 劉建業(yè), 王燕萍. 激光捷聯(lián)慣性導(dǎo)航中不可交換性誤差補償算法研究[J]. 東南大學(xué)學(xué)報:自然科學(xué)版, 2003, 33(4): 419-423. XIONG Zhi, LIU Jian-ye, WANG Yan-ping. Study on noncommutativity errors correction algorithms of laser strapdown inertial navigation system[J]. Journal of Southeast University:Natural Science Edition, 2003, 33(4): 419-423. (in Chinese)
[12] 趙欣, 王仕成, 楊東方,等. 一種改進的高動態(tài)捷聯(lián)慣導(dǎo)解算算法[J]. 中國慣性技術(shù)學(xué)報, 2011, 19(2): 163-169. ZHAO Xin, WANG Shi-cheng, YANG Dong-fang, et al. Improved algorithm for high dynamic strapdown inertial navigation system[J]. Journal of Chinese Inertial Technology, 2011, 19(2): 163-169. (in Chinese)
[13] 秦永元. 慣性導(dǎo)航[M]. 西安: 科學(xué)出版社, 2006. QIN Yong-yuan. Inertial navigation[M]. Xi’an: Science Press, 2006. (in Chinese)
[14] 劉建業(yè), 曾慶化, 趙偉,等. 導(dǎo)航系統(tǒng)理論與應(yīng)用[M]. 西安: 西北工業(yè)大學(xué)出版社, 2010. LIU Jian-ye, ZENG Qing-hua, ZHAO Wei, et al. Theory and application of navigation system[M]. Xi’an: Northwestern Polytechnical University Press, 2010. (in Chinese)
[15] 郁豐. 微小衛(wèi)星姿軌自主確定技術(shù)研究[D]. 南京:南京航空航天大學(xué), 2008. YU Feng. Research on autonomous determination of micro satellite attitude and orbit[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2008. (in Chinese)
A SINS/STAR Integrated Navigation Method Based on Online Estimation of Gyroscope Error in Inertial Coordinate
ZHAO Hui1, XIONG Zhi1, SHI Li-juan1, YU Feng2, LIN Ai-jun1
(1.College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, Jiangsu, China;2.College of Astronautics, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, Jiangsu, China)
In traditional SINS/STAR integrated navigation system, the stellar attitude information needs to be converted from inertial coordinate system to navigation coordinate system. Since the geographic position information needs to be introduced in the conversion process of attitude information, it is inevitably to introduce the conversion error and is unable to take full use of the correction effect of high precision stellar attitude information on the error of inertial navigation system. In consideration that the gyroscope output information and the stellar attitude information can be directly obtained in the inertial reference frame, a SINS/STAR integrated navigation method based on the online estimation of gyroscope error in inertial coordinate is proposed. The drift error of gyro can be estimated online in inertial coordinate by establishing the mathematical model of SINS/STAR integrated navigation based on gyroscope error. The inertial navigation error caused by the drift error of gyro can be effectively reduced by correcting the gyro error in real-time. The simulated results show that the proposed method can effectively estimate the drift error of gyro, and improve the accuracy of inertial navigation system.
control science and technology; SINS/STAR integrated system; gyroscope drift error; inertial coordinate; open loop estimation
2016-03-07
國家自然科學(xué)基金項目(61533009、61374115、61533008、61673208); 江蘇省六大人才高峰資助項目(2013-JY-013); 江蘇高校優(yōu)勢學(xué)科建設(shè)工程項目(2014年); 中央高?;究蒲袠I(yè)務(wù)費專項資金項目(NP2015406、NP2015212、NZ2016104); 南京航空航天大學(xué)大學(xué)研究生創(chuàng)新基地(實驗室)開放基金項目(KFJJ20150315)
趙慧(1990—), 女, 碩士研究生。 E-mail: zhaohuinrc@nuaa.edu.cn; 熊智(1976—), 男, 研究員, 博士生導(dǎo)師。 E-mail: xznrc@nuaa.edu.cn
V249.32+8
A
1000-1093(2016)12-2259-09
10.3969/j.issn.1000-1093.2016.12.011