基于模型預測的無人機組合陀螺在線標定

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

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

0 引 言

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

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

1 SINS/CNS組合在線標定陀螺輸出誤差模型

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

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

(1)

(2)

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

2 模型預測濾波基本算法

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

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

(3)

Z=h(x)+v

(4)

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

(5)

根據李導數定義k階李導數

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

(6)

(7)

(8)

(9)

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

(10)

(11)

(12)

定義性能指標函數

(13)

其中W∈Ru×u是模型誤差加權矩陣。

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

(14)

3 基于模型預測的SINS/CNS組合陀螺在線標定混合濾波方法

3.1 SINS/CNS組合陀螺在線標定方法

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

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

?q=[δq,δe1]T

(15)

由四元數運動學方程有

(16)

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

(17)

(18)

(19)

將式(18)代入(17)得

(20)

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

(22)

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

(23)

(24)

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

(25)

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

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

(26)

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

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

(27)

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

根據模型預測濾波步驟, 應用模型預測濾波算法, 對刻度因數和安裝誤差進行估計。

4 仿真研究

4.1 仿真條件

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

4.2 仿真結果分析

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

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

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

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

5 結 語

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

參考文獻：

[1]張樹俠, 孫靜. 捷聯式慣性導航系統(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組合導航系統(tǒng)的在線標定算法 [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]. 長沙： 國防科技大學航天科學與工程學院, 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]陳少華. 高軌機動航天器SINS/GNSS/CNS自主導航技術研究 [D]. 南京： 南京航空航天大學自動化學院, 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]陳哲. 捷聯慣性導航系統(tǒng)原理 [M]. 北京：中國宇航出版社, 1986.

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

[9]賈鶴鳴, 宋文龍, 牟宏偉, 等. ISR-CDKF在SINS大方位失準角初始對準中的應用 [J]. 吉林大學學報: 信息科學版, 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.