賈鶴鳴, 宋文龍, 牟宏偉, 車延庭

(1. 東北林業(yè)大學(xué) 機(jī)電工程學(xué)院, 哈爾濱 150040； 2. 中國運(yùn)載火箭技術(shù)研究院, 北京 100076；3. 哈爾濱工程大學(xué) 自動(dòng)化學(xué)院, 哈爾濱 150001)

0 引 言

對于捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS: Strap Down Inertial Navigation System), 由于工作時(shí)間長、 環(huán)境等因素的變化, 陀螺漂移會(huì)導(dǎo)致標(biāo)定值產(chǎn)生很大差異, 使系統(tǒng)無法滿足對準(zhǔn)、 導(dǎo)航精度的要求[1,2]。為改善這種狀況, 筆者提出了捷聯(lián)慣性導(dǎo)航系統(tǒng)/天文導(dǎo)航系統(tǒng)(SINS/CNS： SINS/Celestial Navigation System)組合陀螺在線標(biāo)定方法。

捷聯(lián)慣導(dǎo)系統(tǒng)通常利用卡爾曼濾波(KF: Kalman Filter)方法實(shí)現(xiàn)標(biāo)定, 但其精度在很大程度上由系統(tǒng)的能觀測性所決定； 同時(shí), 它需要建立比較準(zhǔn)確的系統(tǒng)狀態(tài)空間模型[3]。而模型預(yù)測濾波(MPF:Model Prediction Filter)是一種基于非線性系統(tǒng)模型的實(shí)時(shí)濾波方法, 克服了卡爾曼濾波的缺點(diǎn), 能在線估計(jì)任何形式的未知模型誤差, 降低了系統(tǒng)的維數(shù), 計(jì)算速度快, 具有良好的魯棒穩(wěn)定性[4]。

1 SINS/CNS組合在線標(biāo)定陀螺輸出誤差模型

陀螺儀長時(shí)間工作, 由于環(huán)境變化等因素會(huì)引起陀螺漂移、 標(biāo)度因數(shù)誤差和陀螺安裝軸不正交誤差等因素的變化, 為更好地估計(jì)陀螺漂移后修正慣性器件誤差, 將陀螺誤差源建入陀螺模型得陀螺測量值

ωg=ω+b+K+τ+ng

(1)

(2)

其中S為刻度因數(shù)誤差陣,ω為陀螺實(shí)際輸出的角速度；Δxy,Δxz,Δyx,Δyz,Δzx,Δzy為陀螺的安裝誤差角。

2 模型預(yù)測濾波基本算法

模型預(yù)測濾波(MPF： Model Prediction Filter)是利用預(yù)測輸出跟蹤測量輸出, 估計(jì)系統(tǒng)的模型誤差, 將模型誤差作為估計(jì)量, 利用觀測量糾正模型中不確定的誤差[5,6]。

假設(shè)一個(gè)非線性系統(tǒng)如下

(3)

Z=h(x)+v

(4)

其中f∈Rn是連續(xù)可微的非線性函數(shù),x∈Rn是狀態(tài)變量,D∈Rq是模型誤差向量,G∈Rn×q是模型誤差分布矩陣。Z∈Rm是量測向量,v是量測噪聲向量, 并假定其為零均值的高斯白噪聲, 協(xié)方差為E{vvT}∈R。

(5)

根據(jù)李導(dǎo)數(shù)定義k階李導(dǎo)數(shù)

缺血性腦血管病好發(fā)于中老年人，近年來發(fā)病率越來越高[1] 。絕大多數(shù)患者患病與頸動(dòng)脈粥樣硬化有著密切關(guān)系。而癥狀性頸動(dòng)脈狹窄是導(dǎo)致動(dòng)脈粥樣硬化的主要病變，針對此病應(yīng)于恰當(dāng)?shù)臅r(shí)間給予外科手術(shù)干預(yù)[2] 。本研究收集2015年7月至2017年11月大連市中心醫(yī)院診治的30例癥狀性頸動(dòng)脈狹窄患者的臨床資料，通過比較頸動(dòng)脈內(nèi)膜切除術(shù)(carotid endarterctomy,CEA)術(shù)前與術(shù)后狹窄率及頸內(nèi)動(dòng)脈收縮期峰值流速，分析其治療效果。

(6)

(7)

(8)

(9)

Λ(Δt)∈Rm×m為對角陣, 其對角元素為

(10)

(11)

(12)

定義性能指標(biāo)函數(shù)

(13)

其中W∈Ru×u是模型誤差加權(quán)矩陣。

假定小的時(shí)間間隔為常數(shù), 則Z(t)=Zk,Z(t+Δt)=Zk+1。為使J最小, 需要滿足條件?J/?Dk=0, 由此可得到[tk,tk+1]時(shí)間區(qū)間內(nèi)的模型誤差估計(jì)

(14)

3 基于模型預(yù)測的SINS/CNS組合陀螺在線標(biāo)定混合濾波方法

3.1 SINS/CNS組合陀螺在線標(biāo)定方法

首先取誤差四元數(shù)的矢量部分和陀螺漂移誤差6維完全可觀測變量為狀態(tài)變量, 以誤差四元數(shù)的矢量部分作為觀測量, 利用卡爾曼濾波算法估計(jì)陀螺漂移； 將估計(jì)的陀螺漂移平均值建入陀螺誤差模型, 再取誤差四元數(shù)的矢量部分和陀螺漂移平均值共6維變量為狀態(tài)變量, 刻度因數(shù)估計(jì)誤差和安裝誤差估計(jì)誤差共9維變量為模型誤差向量, 誤差四元數(shù)的矢量部分為觀測量, 利用模型預(yù)測濾波估計(jì)陀螺刻度因數(shù)誤差和安裝誤差。

3.2 基于模型預(yù)測混合濾波方法的狀態(tài)方程和量測方程

?q=[δq,δe1]T

(15)

由四元數(shù)運(yùn)動(dòng)學(xué)方程有

(16)

對式(15)求導(dǎo), 并將式(16)代入得

(17)

(18)

(19)

將式(18)代入(17)得

(20)

將式(19)和式(21)代入式(20)后對其線性化, 得

(22)

將式(2)代入式(22), 寫成矩陣形式為

(23)

(24)

根據(jù)模型預(yù)測濾波算法, 系統(tǒng)狀態(tài)方程為

(25)

其中F∈R6×6是系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,X∈R6是狀態(tài)變量,G1∈R6×6是模型誤差分布陣,D∈R9是模型誤差向量,G2∈R6×6是系統(tǒng)噪聲驅(qū)動(dòng)陣,W∈R6是系統(tǒng)噪聲。

取δq1的矢量部分δe作為觀測量

(26)

系統(tǒng)量測方程為

Z(t)=HX(t)+V(t)

(27)

根據(jù)第2節(jié)中模型預(yù)測濾波基本算法, 取pi=1,n=6,u=3,m=9, 可得

根據(jù)模型預(yù)測濾波步驟, 應(yīng)用模型預(yù)測濾波算法, 對刻度因數(shù)和安裝誤差進(jìn)行估計(jì)。

4 仿真研究

4.1 仿真條件

假設(shè)無人機(jī)的初始位置為東經(jīng)126.63°, 北緯47.75°, 高度是5 000 m； 載體勻速飛行且飛行速度為150 m/s, 飛機(jī)做航向變化的姿態(tài)機(jī)動(dòng), 1 200 s方位角改變60°； 0～10 min采用KF估計(jì)陀螺漂移, 10～20 min切換濾波方式, 采用MPF估計(jì)刻度因數(shù)和安裝誤差； 初始航向角為30°, 初始俯仰角為0, 初始橫滾角為0； 方位失準(zhǔn)角5角分, 水平失準(zhǔn)角20角秒； 陀螺常值漂移為0.01°/h； 刻度因數(shù)誤差為50×10-3‰； 安裝誤差為3角分； 仿真時(shí)間為1 200 s, 采樣周期為1 s。

4.2 仿真結(jié)果分析

安裝偏差角的估計(jì)結(jié)果只給出Δzx,Δzy,Δyz的情況。從圖1～圖3可看出, 經(jīng)過SINS/CNS組合在線標(biāo)定, 陀螺儀的零偏標(biāo)定誤差在萬分之幾度/小時(shí)級別上, 陀螺儀的刻度因數(shù)誤差小于3×10-3‰, 陀螺儀的安裝誤差角誤差也小于0.06角分, 估計(jì)結(jié)果均收斂于設(shè)置值, 標(biāo)定結(jié)果比較理想。由圖2和圖3可看出, 10 min后切換濾波方式, 利用模型預(yù)測濾波有效地估計(jì)了陀螺刻度因數(shù)和安裝誤差, 收斂速度快, 同時(shí)保證了濾波的數(shù)值穩(wěn)定性。

圖1 模型預(yù)測混合濾波陀螺漂移仿真曲線

圖2 模型預(yù)測混合濾波陀螺刻度因數(shù)仿真曲線

圖3 模型預(yù)測混合濾波陀螺安裝誤差仿真曲線

5 結(jié) 語

筆者對KF-MPF混合濾波在SINS/CNS組合陀螺在線標(biāo)定的應(yīng)用進(jìn)行了研究, 分析了MPF的數(shù)學(xué)機(jī)理。實(shí)驗(yàn)仿真結(jié)果表明, KF-MPF混合濾波能很好地適用于在線標(biāo)定中, 避免了線性化的局限, 而且不需要建立準(zhǔn)確的狀態(tài)空間模型, 提高了可靠性。同時(shí), KF-MPF混合濾波可減少計(jì)算量, 保證了數(shù)值穩(wěn)定性。結(jié)果證明了KF-MPF混合濾波在陀螺在線標(biāo)定中的可行性和優(yōu)越性, 具有工程應(yīng)用價(jià)值。

參考文獻(xiàn)：

[1]張樹俠, 孫靜. 捷聯(lián)式慣性導(dǎo)航系統(tǒng) [M]. 北京： 國防工業(yè)出版社, 1992： 6-10.

ZHANG Shu-xia, SUN Jing. Strap Down Inertial Navigation System [M]. Beijing: National Defence Industrial Press, 1992： 6-10.

[2]白俊卿, 衛(wèi)育新. 車載SINS/GPS組合導(dǎo)航系統(tǒng)的在線標(biāo)定算法 [J]. 電子設(shè)計(jì)工程, 2010, 18(2)： 89-90.

BAI Jun-qing, WEI Yu-xin. Arithmetic of Vehicular SINS/GPS Integrated Navigation System’s On-Line Calibration [J]. Electronic Design Engineering, 2010, 18(2)： 89-90.

[3]CHOUKROUN D, WEISS H, ITZHACK Y, et al. Kalman Filtering for Matrix Estimation [J]. IEEE Transactions on Aerospace and Electronic System, 2006, 42(1)： 147-159.

[4]鄭慶輝. 基于GPS的航天器姿態(tài)、 相對姿態(tài)確定研究 [D]. 長沙： 國防科技大學(xué)航天科學(xué)與工程學(xué)院, 2003.

ZHENG Qing-hui. GPS Based Absolute and Relative Attitude Determination of Spacecraft [D]. Changsha: College of Space Science and Engineering, National University of Defense Technology, 2003.

[5]LU Ping. Nonlinear Predictive Controllers for Continuous Systems [J]. Journal of Guidance Control and Dynamics, 1994, 17(3)： 553-560.

[6]LU Ping. Optimal Predictive Control of Continuous Nonlinear Systems [J]. International Journal of Control, 1995, 62(3)： 633-649.

[7]陳少華. 高軌機(jī)動(dòng)航天器SINS/GNSS/CNS自主導(dǎo)航技術(shù)研究 [D]. 南京： 南京航空航天大學(xué)自動(dòng)化學(xué)院, 2012.

CHEN Shao-hua. Research on Autonomous SINS/GNSS/CNS Navigation for High Orbit Vehicle Transfering [D]. Nanjing: College of Automation, Nanjing University Aeronautics and Astronautics, 2012.

[8]陳哲. 捷聯(lián)慣性導(dǎo)航系統(tǒng)原理 [M]. 北京：中國宇航出版社, 1986.

CHEN Zhe. Strap Down Inertial Navigation System Principle [M]. Beijing: China Astronautics Publishing House, 1986.

[9]賈鶴鳴, 宋文龍, 牟宏偉, 等. ISR-CDKF在SINS大方位失準(zhǔn)角初始對準(zhǔn)中的應(yīng)用 [J]. 吉林大學(xué)學(xué)報(bào): 信息科學(xué)版, 2013, 31(2)： 196-202.

JIA He-ming, SONG Wen-long, MU Hong-wei, et al. Application of ISR-CDKF in Initial Alignment of Large Azimuth Misalignment in the SINS [J]. Journal of Jilin University: Information Science Edition, 2013, 31(2)： 196-202.