平滑變結(jié)構(gòu)濾波在航天器組合導(dǎo)航中的應(yīng)用

施常勇，張麗敏

（1. 上海航天控制技術(shù)研究所，上海 201109；2. 上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海 201109）

平滑變結(jié)構(gòu)濾波在航天器組合導(dǎo)航中的應(yīng)用

施常勇1,2，張麗敏1,2

（1. 上海航天控制技術(shù)研究所，上海 201109；2. 上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海 201109）

針對(duì)系統(tǒng)模型不準(zhǔn)確或噪聲無(wú)法建模等導(dǎo)致的組合導(dǎo)航精度下降問(wèn)題，在閉環(huán)間接濾波方案中引入一種基于變結(jié)構(gòu)和滑模控制概念的平滑變結(jié)構(gòu)濾波器（SVSF），該濾波器對(duì)于建模的不確定性和給定上限但無(wú)法建模的擾動(dòng)噪聲具有較好的穩(wěn)定性和魯棒性。將提出的方案應(yīng)用于航天器SINS/GNSS組合導(dǎo)航系統(tǒng)中，進(jìn)行仿真驗(yàn)證，并與EKF濾波器進(jìn)行比較。仿真結(jié)果表明：SVSF盡管不是“最優(yōu)”濾波器，但其精度與EKF相當(dāng)，可實(shí)現(xiàn)位置精度優(yōu)于10 m，速度精度優(yōu)于0.01 m/s；在模型誤差與測(cè)量誤差增大和系統(tǒng)突變等情況下，其較EKF更加具有抗干擾能力，可提高導(dǎo)航系統(tǒng)的魯棒性和穩(wěn)定性。

平滑變結(jié)構(gòu)濾波器；組合導(dǎo)航；魯棒性；航天器

隨著空間技術(shù)的發(fā)展，人們對(duì)航天器自主導(dǎo)航的需求越來(lái)越高，GNSS/SINS組合導(dǎo)航系統(tǒng)充分利用了SINS短期精度高、抗干擾能力強(qiáng)和GNSS長(zhǎng)期精度高的優(yōu)點(diǎn)，取長(zhǎng)補(bǔ)短，實(shí)現(xiàn)航天器高精度高可靠自主導(dǎo)航[1]。在組合導(dǎo)航系統(tǒng)中常采用卡爾曼濾波技術(shù)[2]，但是該方法要求能夠提供系統(tǒng)精確的狀態(tài)方程和量測(cè)方程，并且系統(tǒng)噪聲和量測(cè)噪聲是均值為零、協(xié)方差已知的高斯噪聲，否則難以滿足要求，甚至發(fā)散。針對(duì)此問(wèn)題，不同的研究者提出了如 EKF[3]、UKF[4]、PF[5]或CKF[6]等濾波算法，但仍存在諸多不足，如：模型誤差作為過(guò)程噪聲來(lái)處理，且假設(shè)為高斯白噪聲，這與組合導(dǎo)航系統(tǒng)的實(shí)際噪聲情況并不相符；對(duì)系統(tǒng)模型不確定性的魯棒性差等。

平滑變結(jié)構(gòu)濾波器（smooth variable structure filter，SVSF）是由Habibi等人于2007年在變結(jié)構(gòu)濾波器 VSF（variable structure filter，2002）[7]基礎(chǔ)上提出的，其基本思想是基于變結(jié)構(gòu)和滑模控制概念，采用變結(jié)構(gòu)的增益，使得預(yù)測(cè)狀態(tài)趨近于系統(tǒng)真實(shí)軌跡，因此是一種“預(yù)測(cè)-校正”的估計(jì)器，其對(duì)于建模的不確定性和給定上限但無(wú)法建模的擾動(dòng)噪聲具有較好的穩(wěn)定性和魯棒性[7]。本文將結(jié)合 SVSF理論，研究其在航天器組合導(dǎo)航中應(yīng)用。

1 平滑變結(jié)構(gòu)濾波器

設(shè)系統(tǒng)方程為：

SVSF估計(jì)的基本概念如圖1所示，假設(shè)某一狀態(tài)量為x，其估計(jì)值為，其中的初始值選取一般基于概率分布或先驗(yàn)知識(shí)。通過(guò)使用SVSF增益，將被強(qiáng)制到狀態(tài)軌跡周圍（也稱存在子空間）內(nèi)， 一旦進(jìn)入存在子空間，將沿系統(tǒng)狀態(tài)軌跡的切換。為減小切換抖動(dòng)，在該區(qū)域中使用飽和項(xiàng)來(lái)使結(jié)果平滑。當(dāng)不是所有的動(dòng)態(tài)都被明確建模時(shí)，該策略具有較強(qiáng)的魯棒性。

圖1 SVSF狀態(tài)估計(jì)原理圖Fig.1 Principle of SVSF state estimation

證明：根據(jù)Lyapunov穩(wěn)定性理論，定義Lyapunov函數(shù)如下：

若ΔV＜0，則認(rèn)為系統(tǒng)估計(jì)過(guò)程是穩(wěn)定的，由此：

基于上述前提，可以推導(dǎo)出標(biāo)準(zhǔn)變結(jié)構(gòu)濾波器的增益方程：

ψi為給定的噪聲上限值。

式(5)中可以體現(xiàn)SVSF的核心思想，利用到噪聲信息和誤差信息，在誤差上界處（滑模面處）采用飽和項(xiàng)強(qiáng)制估計(jì)狀態(tài)進(jìn)行切換，在上下滑模面內(nèi)采用誤差新息進(jìn)行更新。

經(jīng)過(guò)數(shù)年的發(fā)展，SVSF出現(xiàn)了多種形式，如固定邊界層厚度[9]、基于協(xié)方差的最優(yōu)邊界層厚度[10]，也出現(xiàn)了SVSF與CKF[11]、UKF[12]、PF[13]組合濾波的研究，本文采用基于協(xié)方差的非線性系統(tǒng)迭代方程[14]：

狀態(tài)預(yù)測(cè)方程：

協(xié)方差預(yù)報(bào)方程：

誤差預(yù)報(bào)：

增益方程：

系統(tǒng)狀態(tài)估計(jì)方程：

協(xié)方差推廣方程：

誤差方程：

式(7)和(11)中Qk和Rk分別為系統(tǒng)和量測(cè)噪聲協(xié)方差陣。

對(duì)于非線性動(dòng)力學(xué)方程，需進(jìn)行線性化處理：

2 SINS/GNSS 組合導(dǎo)航系統(tǒng)設(shè)計(jì)

在 SINS/GNSS組合導(dǎo)航系統(tǒng)中，可采用多種濾波結(jié)構(gòu)形式[15]，如開(kāi)環(huán)直接濾波、閉環(huán)直接濾波、開(kāi)環(huán)間接濾波或閉環(huán)間接濾波等。本文采用閉環(huán)間接濾波方案，將SINS和GNSS子系統(tǒng)各自確定的航天器相對(duì)于J2000系的位置和速度相減作為量測(cè)，送入濾波器進(jìn)行濾波計(jì)算，從而獲得導(dǎo)航參數(shù)誤差的最優(yōu)估計(jì)值；利用濾波估計(jì)值實(shí)時(shí)地對(duì)SINS進(jìn)行誤差校正；最后，將校正后的導(dǎo)航結(jié)果作為組合導(dǎo)航系統(tǒng)的輸出。SINS/GNSS組合導(dǎo)航方案結(jié)構(gòu)如圖2所示。

圖2 SINS/GNSS組合導(dǎo)航系統(tǒng)框圖Fig.2 Architecture of SINS/GNSS integrated navigation system

選擇J2000慣性坐標(biāo)系i作為導(dǎo)航坐標(biāo)系，利用航天器本體系到慣性系的轉(zhuǎn)換矩陣將加速度計(jì)的輸出從航天器本體坐標(biāo)系變換到慣性坐標(biāo)系即：

則航天器在慣性坐標(biāo)系的加速度為：

注意到地球引力項(xiàng)gni與當(dāng)前的位置是相關(guān)的，而當(dāng)前的位置又是未知的，因此計(jì)算時(shí)將上一時(shí)刻近似代替當(dāng)前時(shí)刻gni，則慣導(dǎo)位置求解公式為：

其中，Tzh為SINS解算周期。

慣導(dǎo)系統(tǒng)速度求解：為了提高系統(tǒng)的導(dǎo)航精度，在求解慣導(dǎo)系統(tǒng)速度時(shí)，當(dāng)前時(shí)刻的地球引力項(xiàng)gni用該Tzh內(nèi)平均值來(lái)近似代替，則慣導(dǎo)速度求解公式為：

隨著生活方式的改變，我國(guó)人群中存在MS的比例逐年增加，目前已報(bào)道的我國(guó)成人MS的比例為9.8%～17.8%[17]，本研究中合并MS的前列腺癌患者占16.08%，與之前的報(bào)道相符。ZHANG等[12]的研究結(jié)果顯示MS和前列腺癌的Gleason評(píng)分≥8以及較晚的病理分期有關(guān)。本研究的結(jié)果證實(shí)了MS和病理T分期的相關(guān)性，然而，未發(fā)現(xiàn)兩組之間Gleason評(píng)分有明顯差異，可能的原因是本研究采用的Gleason評(píng)分分組方式與之前的研究不同。為了減少其他臨床終點(diǎn)對(duì)結(jié)果的干擾，我們采用競(jìng)爭(zhēng)風(fēng)險(xiǎn)模型評(píng)估MS對(duì)生化復(fù)發(fā)的影響，結(jié)果顯示合并MS的前列腺癌患者出現(xiàn)生化復(fù)發(fā)的概率是無(wú)MS者的1.86倍。

GNSS的輸出為：

其中，δv為速度殘差，δr為位置殘差。

選擇慣導(dǎo)輸出的航天器J2000坐標(biāo)系下的速度、位置與GNSS輸出的相應(yīng)值之差作為量測(cè)，構(gòu)造的量測(cè)為：

量測(cè)方程為：

由此可知，系統(tǒng)的測(cè)量矩陣為單位陣，即：

3 仿真研究

為驗(yàn)證SVSF在GNSS/SINS組合導(dǎo)航中的可用性和適應(yīng)性，采用高精度軌道動(dòng)力學(xué)設(shè)計(jì)了一段空間飛行軌跡，陀螺、加速度計(jì)和GNSS的誤差設(shè)置見(jiàn)3.1節(jié)，分別對(duì)正常誤差工況、過(guò)程異常和敏感器噪聲增大等三種工況進(jìn)行了仿真，并將SVSF與EKF進(jìn)行了對(duì)比驗(yàn)證。

3.1 敏感器誤差設(shè)計(jì)

組合仿真周期：TZH=0.5 s。

3.2 仿真結(jié)果

1）正常誤差工況

采用表1中設(shè)置的敏感器誤差參數(shù)，SVSF與EKF仿真曲線如圖3所示。從圖中可以看出，盡管SVSF不是“最優(yōu)”濾波器，但兩種濾波器的誤差基本在同一量級(jí)，位置精度優(yōu)于10 m，速度精度優(yōu)于0.01 m/s。

圖3 位置速度誤差曲線(正常誤差工況)Fig.3 True estimation errors of position and velocity (normal errors of sensors)

2）過(guò)程異常工況

為驗(yàn)證SVSF的魯棒性，在500s時(shí)GNSS的Y軸速度噪聲異常放大100倍(持續(xù)50s)，SVSF與EKF在速度上均受到了影響，但由于SVSF是基于變結(jié)構(gòu)的思想，具有較好的魯棒性，其在位置維的影響遠(yuǎn)小于EKF，仿真結(jié)果如圖4所示。

圖4 過(guò)程異常工況位置速度誤差曲線Fig.4 True estimation errors of position and velocity (abnormity states)

3）敏感器噪聲增大工況

為進(jìn)一步驗(yàn)證SVSF的性能，在GNSS正常噪聲設(shè)置的基礎(chǔ)上，對(duì)速度誤差放大10倍處理，按照原有設(shè)計(jì)參數(shù)，SVSF表現(xiàn)出較好的適應(yīng)性，濾波穩(wěn)定性優(yōu)于EKF濾波器，濾波精度也較EKF的高，仿真曲線如圖5所示。

圖5 初始噪聲增大工況位置速度誤差曲線Fig.5 True estimation error of position and velocity (large errors of sensors)

4 結(jié) 論

本文針對(duì)組合導(dǎo)航系統(tǒng)中系統(tǒng)模型不準(zhǔn)確或噪聲無(wú)法建模導(dǎo)致的導(dǎo)航精度下降問(wèn)題，研究了 SVSF濾波器在航天器 GNSS/SINS組合導(dǎo)航系統(tǒng)中的應(yīng)用技術(shù)。針對(duì)航天器軌道運(yùn)行的特點(diǎn)，提出了閉環(huán)間接濾波方案，并搭建了仿真模型，對(duì)所設(shè)計(jì)的導(dǎo)航系統(tǒng)進(jìn)行了仿真驗(yàn)證，并將其與EKF進(jìn)行了對(duì)比分析。仿真結(jié)果表明，SVSF具有較好的魯棒性和穩(wěn)定性，具有較強(qiáng)的工程應(yīng)用價(jià)值。

（References）：

[1]魏偉, 武云云. 慣性/天文/衛(wèi)星組合導(dǎo)航技術(shù)的現(xiàn)狀與展望[J]. 現(xiàn)代導(dǎo)航, 2014(1): 63-65.Wei Wei, Wu Yun-yun. Status and prospect of INS/CNS/GNSS integrated navigation technology[J]. Modern Navigation, 2014(1): 63-65.

[2]謝木生, 卡爾曼濾波在GPS/INS 組合導(dǎo)航系統(tǒng)中的應(yīng)用研究[D]. 中南大學(xué), 2013: 19-26.Xie Mu-sheng. Study on application of Kalman in INS/GPS integrated navition[D]. Central South University,2013: 19-26.

[3]吳春光, 李雙明, 潘玉純. EKF和 UKF在非線性組合導(dǎo)航系統(tǒng)中的對(duì)比研究[J]. 長(zhǎng)春理工大學(xué)學(xué)報(bào) (自然科學(xué)版), 2016, 39(2): 82-85.Wu Chun-guang, Li Shuang-ming, Pan Yu-chun. Comparison of EKF and UKF applied in nonlinear navigation system[J]. Journal of Changchun University of Science and Technology (Natural Science Edition), 2016, 39(2): 82-85.

[4]胡高歌, 高社生, 趙巖. 一種新的自適應(yīng) UKF 算法及其在組合導(dǎo)航中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2014,22(3): 357-360.Hu Gao-ge, Gao She-sheng, Zhao Yan. Novel adaptive UKF and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(3): 357-360.

[5]薛麗, 高社生, 胡高歌. 自適應(yīng) Sage-Husa 粒子濾波及其在組合導(dǎo)航中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào),2013, 21(1): 84-88.Xue Li, Gao She-sheng, Hu Gao-ge. Adaptive Sage-Husa particle filtering and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2013,21(1): 84-88.

[6]孫楓, 唐李軍. 基于cubature Kalman filter的INS/GPS組合導(dǎo)航濾波算法[J]. 控制與決策, 2012, 27(7): 1032-1036.Sun Feng, Tang Li-jun, INS/GPS integrated navigation filter algorithm based on cubature Kalman filter[J]. Control and Decision, 2012, 27(7): 1032-1036.

[7]Habibi S, Burton R, Chinniah Y. Estimation using a new variable structure filter[C]//Proceedings of the 2002 American Control Conference. 2002, vol.4: 2937-2942.

[8]Habibi S. The smooth variable structure filter[J]. Proceedings of the IEEE, May 2007, 95(5): 1026-1059.

[9]Gadsden S A, Habibi, Kirubarajan T. Kalman and smooth variable structure filters for robust estimation[J]. IEEE Transactions on Aerospace and Electronic Systems, 2014,50(2): 1038-1050.

[10]Gadsden S A, Habibi S R. A new form of the smooth variable structure filter with a covariance derivation[C]//49th IEEE Conference on Decision and Control. 2010:7389-7394.

[11]Al-Shabi M, Gadsden S A, Habibi S R. Kalman filtering strategies utilizing the chattering effects of the smooth variable structure filter[J]. Signal Processing, 2013, 93(2):420-431.

[12]Al-Shabi M, Hatamleh K S. The unscented smooth variable structure filter application into a robotic arm[C]//ASME 2014 International Mechanical Engineering Congress and Exposition. 2014.

[13]Gadsden S A, Habibi S R, Kirubarajan T. The smooth particle variable structure filter[J]. Transactions of the Canadian Society for Mechanical Engineering, 2012,36(2): 177-193.

[14]Gadsden S A, Sayed M E, Habibi S R. Derivation of an optimal boundary layer width for the smooth variable structure filter[C]//American Control Conference. 2011:4922-4927.

[15]Benzerrouk H, Nebylov A V. Aircraft’s approach integrated navigation system INS/GNSS based on joint application of linear and nonlinear filtering[C]//the 18th IFAC World Congress. Milano, Italy, 2011: 1866-1871.

Application of SVSF in spacecraft integrated navigation system

SHI Chang-yong1,2, ZHANG Li-min1,2
(1. Shanghai Institute of Spaceflight Control Technology, Shanghai 201109, China;2. Shanghai Key Laboratory of Aerospace Intelligent Control Technology, Shanghai 201109, China)

In view of the precision degradation of GNSS/SINS integrated navigation system due to the fact that the system model is not accurate or the noises can not be modeled, a smooth variable structure filter(SVSF) based on variable structure and sliding mode control concept is introduced in the closed loop indirect filtering scheme. The filter is stable and robust for the modeling uncertainties and the disturbance noise when with an upper bound on the level of un-modeled dynamics or knowledge of the level of noise.By applying the proposed scheme into the spacecraft SINS/GNSS integrated navigation system, the simulation is carried out and the comparison with the EKF filter is made. The simulation results show that the SVSF, though suboptimal, has equivalent precision to EKF, and the position and velocity accuracies are better than 10 m and 0.01 m/s, respectively. The SVSF’s anti-interference ability is stronger than that of EKF in cases such as the model and the measurement have larger errors, the system has a mutation, etc.,which can improve the robustness and stability of the navigation system.

smooth variable structure filter; integrated navigation; robustness;spacecraft

1005-6734(2017)04-0478-05

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

U666.1

A

2017-05-26；

2017-07-26

施常勇（1982—），男，高級(jí)工程師，從事航天器導(dǎo)航系統(tǒng)設(shè)計(jì)與研究。E-mail: shi05cy@163.com