平滑變結構濾波在航天器組合導航中的應用

施常勇，張麗敏

（1. 上海航天控制技術研究所，上海 201109；2. 上海市空間智能控制技術重點實驗室，上海 201109）

平滑變結構濾波在航天器組合導航中的應用

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

（1. 上海航天控制技術研究所，上海 201109；2. 上海市空間智能控制技術重點實驗室，上海 201109）

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

平滑變結構濾波器；組合導航；魯棒性；航天器

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

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

1 平滑變結構濾波器

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

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

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

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

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

基于上述前提，可以推導出標準變結構濾波器的增益方程：

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

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

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

狀態(tài)預測方程：

協方差預報方程：

誤差預報：

增益方程：

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

協方差推廣方程：

誤差方程：

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

對于非線性動力學方程，需進行線性化處理：

2 SINS/GNSS 組合導航系統(tǒng)設計

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

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

選擇J2000慣性坐標系i作為導航坐標系，利用航天器本體系到慣性系的轉換矩陣將加速度計的輸出從航天器本體坐標系變換到慣性坐標系即：

則航天器在慣性坐標系的加速度為：

注意到地球引力項gni與當前的位置是相關的，而當前的位置又是未知的，因此計算時將上一時刻近似代替當前時刻gni，則慣導位置求解公式為：

其中，Tzh為SINS解算周期。

慣導系統(tǒng)速度求解：為了提高系統(tǒng)的導航精度，在求解慣導系統(tǒng)速度時，當前時刻的地球引力項gni用該Tzh內平均值來近似代替，則慣導速度求解公式為：

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

GNSS的輸出為：

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

選擇慣導輸出的航天器J2000坐標系下的速度、位置與GNSS輸出的相應值之差作為量測，構造的量測為：

量測方程為：

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

3 仿真研究

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

3.1 敏感器誤差設計

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

3.2 仿真結果

1）正常誤差工況

采用表1中設置的敏感器誤差參數，SVSF與EKF仿真曲線如圖3所示。從圖中可以看出，盡管SVSF不是“最優(yōu)”濾波器，但兩種濾波器的誤差基本在同一量級，位置精度優(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）過程異常工況

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

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

3）敏感器噪聲增大工況

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

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

4 結 論

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

（References）：

[1]魏偉, 武云云. 慣性/天文/衛(wèi)星組合導航技術的現狀與展望[J]. 現代導航, 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 組合導航系統(tǒng)中的應用研究[D]. 中南大學, 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在非線性組合導航系統(tǒng)中的對比研究[J]. 長春理工大學學報 (自然科學版), 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]胡高歌, 高社生, 趙巖. 一種新的自適應 UKF 算法及其在組合導航中的應用[J]. 中國慣性技術學報, 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]薛麗, 高社生, 胡高歌. 自適應 Sage-Husa 粒子濾波及其在組合導航中的應用[J]. 中國慣性技術學報,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組合導航濾波算法[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—），男，高級工程師，從事航天器導航系統(tǒng)設計與研究。E-mail: shi05cy@163.com