2.793

                    2018影響因子

                    (CJCR)

                    • 中文核心
                    • EI
                    • 中國科技核心
                    • Scopus
                    • CSCD
                    • 英國科學文摘

                    留言板

                    尊敬的讀者、作者、審稿人, 關于本刊的投稿、審稿、編輯和出版的任何問題, 您可以本頁添加留言。我們將盡快給您答復。謝謝您的支持!

                    姓名
                    郵箱
                    手機號碼
                    標題
                    留言內容
                    驗證碼

                    飽和約束測量擴張狀態濾波與無拖曳衛星位姿自抗擾控制

                    楊飛 談樹萍 薛文超 郭金 趙延龍

                    楊飛, 談樹萍, 薛文超, 郭金, 趙延龍. 飽和約束測量擴張狀態濾波與無拖曳衛星位姿自抗擾控制. 自動化學報, 2020, 45(x): 1?14. doi: 10.16383/j.aas.c190515
                    引用本文: 楊飛, 談樹萍, 薛文超, 郭金, 趙延龍. 飽和約束測量擴張狀態濾波與無拖曳衛星位姿自抗擾控制. 自動化學報, 2020, 45(x): 1?14. doi: 10.16383/j.aas.c190515
                    Yang Fei, Tan Shu-Ping, Xue Wen-Chao, Guo Jin, Zhao Yan-Long. Extended state filtering with saturation-constrainted observations and active disturbance rejection control of position and attitude for drag-free satellites. Acta Automatica Sinica, 2020, 45(x): 1?14. doi: 10.16383/j.aas.c190515
                    Citation: Yang Fei, Tan Shu-Ping, Xue Wen-Chao, Guo Jin, Zhao Yan-Long. Extended state filtering with saturation-constrainted observations and active disturbance rejection control of position and attitude for drag-free satellites. Acta Automatica Sinica, 2020, 45(x): 1?14. doi: 10.16383/j.aas.c190515

                    飽和約束測量擴張狀態濾波與無拖曳衛星位姿自抗擾控制


                    DOI: 10.16383/j.aas.c190515
                    詳細信息
                      作者簡介:

                      北京科技大學碩士研究生. 主要從事自抗擾控制理論及應用等方面的研究.E-mail: changeandbebetter@163. com

                      北京控制工程研究所高級工程師. 主要從事航天器控制理論與控制工程等方面的研究.E-mail: sptan@amss.ac.cn

                      中國科學院數學與系統科學研究院副研究員. 主要從事非線性不確定系統控制與濾波、自抗擾控制等.E-mail: wenchaoxue@amss.ac.cn

                      北京科技大學自動化學院教授. 主要從事集值系統、信息物理系統的辨識與控制等方面的研究. 本文通信作者.E-mail: guojin@ustb.edu.cn

                      中國科學院數學與系統科學研究院研究員. 主要研究方向為系統建模與辨識、集值系統辨識與控制、金融系統建模與分析等.E-mail: ylzhao@amss.ac.cn

                    • 1如果\begin{document}$ {{a}}_k $\end{document}是一個向量, 那么用\begin{document}$ {{a}}_{k,i} $\end{document} 表示其第\begin{document}$ i $\end{document}個分量.2算法1與文[23]中的形式有所不同, 這主要是為了方便后面的信息融合算法設計.
                    • 基金項目:  國家重點研發計劃(2018YFA0703800), 國家自然科學基金項目(61773054, 61633003-3)資助

                    Extended State Filtering with Saturation-Constrainted Observations and Active Disturbance Rejection Control of Position and Attitude for Drag-Free Satellites

                    More Information
                    • Fund Project:  Supported by National Key R&D Program of China under Grant 2018YFA0703800 and the National Natural Science Foundation of China (61773054, 61633003-3)
                    • 摘要: 無拖曳衛星的本體姿態、衛星本體與測試質量間的相對位移及相對姿態的聯合控制受到外部擾動、輸入噪聲、測量噪聲及飽和約束、輸入耦合以及狀態耦合等因素的影響, 控制器的設計面臨挑戰. 本文采用基于擴張狀態的卡爾曼濾波對系統狀態和系統擾動進行實時估計, 引入自抗擾控制策略進行了控制器設計. 針對無拖曳控制子系統設計了測量飽和受限下的擴張狀態估計算法, 并進行了信息融合. 在設計控制律時不僅考慮了對外部擾動的補償, 還將系統狀態間的耦合關系看成內部擾動進行補償, 使得被控系統等價為“積分串聯型系統”, 在此基礎上實現了無拖曳衛星的聯合控制. 數值仿真驗證了方法的有效性和合理性.
                      1如果$ {{a}}_k $是一個向量, 那么用$ {{a}}_{k,i} $ 表示其第$ i $個分量.2算法1與文[23]中的形式有所不同, 這主要是為了方便后面的信息融合算法設計.
                    • 圖  1  無拖曳衛星控制系統圖

                      Fig.  1  Diagram of drag-free satellite control system

                      圖  2  自抗擾控制器框圖

                      Fig.  2  Structure of active disturbance rejection controller

                      圖  3  飽和約束測量

                      Fig.  3  Saturation-constrainted observations

                      圖  4  融合算法示意圖

                      Fig.  4  Diagram of the fusion algorithm

                      圖  5  算法2對擾動估計效果

                      Fig.  5  Disturbance estimation performance of Algorithm 2

                      圖  6  位移/速度的估計效果: 算法1 vs. 算法2

                      Fig.  6  Estimation of displacement/speed: Algorithm 1 vs. Algorithm 2

                      圖  7  擾動估計效果: 算法1 vs.算法2

                      Fig.  7  Estimation of disturbance: Algorithm 1 vs. Algorithm 2

                      圖  8  擾動估計效果: 融合算法3 vs. 算法1

                      Fig.  8  Estimation of disturbance: Algorithm 3 (fusion algorithm) vs. Algorithm 1

                      圖  9  X方向上控制效果對比

                      Fig.  9  Comparison of control performance on X direction

                      圖  10  系統穩定運行時控制效果

                      Fig.  10  Control performance when the system is running steadily

                      圖  11  姿態調整效果

                      Fig.  11  Results of attitude adjustment

                      圖  12  測試質量殘余加速度功率譜密度

                      Fig.  12  Power spectral density of the test mass's residual acceleration

                      表  1  ${ {\Upsilon}}$矩陣

                      Table  1  The matrix of ${ {\Upsilon}}$

                      X方向 Y方向 Z方向
                      $\left[ {\begin{array}{*{20}{c}} 1&0&0 \end{array}} \right]$ $\left[ {\begin{array}{*{20}{c}} 0&1&0 \end{array}} \right]$ $\left[ {\begin{array}{*{20}{c}} 0&0&1 \end{array}} \right]$
                      下載: 導出CSV

                      表  2  系統仿真參數

                      Table  2  System parameters in the simulation

                      變量數值
                      ${m_{{\rm{tm}}}}$1 kg
                      ${m_{{\rm{sc}}}}$1 050 kg
                      ${I_{{\rm{tm}}}}$$0.2667 \times {10^{ - 3}}{\mathbb{I}_3}({\rm{kg}} \cdot {{\rm{m}}^{\rm{2}}})$
                      ${I_{{\rm{sc}}}}$$\left[ {\begin{array}{*{20}{c}} {200}&1&2\\ 1&{2\;700}&1\\ 2&1&{2\;650} \end{array}} \right]({\rm{kg}} \cdot {{\rm{m}}^{\rm{2}}})$
                      ${K_{{\rm{trans}}}}$$\left[ {\begin{array}{*{20}{c}} 1&{0.039}&{0.039}\\ {0.039}&1&{0.039}\\ {0.039}&{0.039}&1 \end{array}} \right] \times {10^{ - 6}}{\rm{(N/m)}}$
                      ${D_{{\rm{trans}}}}$$1.4 \times {10^{ - 11}}{\mathbb{I}_3}({\rm{N}} \cdot {\rm{m}} \cdot {\rm{s/rad}})$
                      ${K_{{\rm{rot}}}}$$\left[ {\begin{array}{*{20}{c}} 1&{10}&{10}\\ {10}&1&{10}\\ {10}&{10}&1 \end{array}} \right] \times {10^{ - 9}}({\rm{N}} \cdot {\rm{m/rad}})$
                      ${D_{{\rm{rot}}}}$$1.4 \times {10^{ - 11}}{I_3}({\rm{N}} \cdot {\rm{m}} \cdot {\rm{s/rad}})$
                      ${{{T}}_{Dsc}}$$ \left[ {\begin{array}{*{20}{c} } { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{t} })}\\ { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{ + } }\dfrac{ {2{\text{π}} } }{3})}\\ { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{ + } }\dfrac{ {4{\text{π}} } }{3})} \end{array} } \right]({\rm{mN} } \cdot {\rm{m} })$
                      ${{{F}}_{Dsc}}$$ \left[ {\begin{array}{*{20}{c} } { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{t} })}\\ { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{ + } }\dfrac{ {2{\text{π}} } }{3})}\\ { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{ + } }\dfrac{ {4{\text{π}} } }{3})} \end{array} } \right]({\rm{mN} })t]({\rm{mN} })$
                      ${{{T}}_{Dtm}}$0
                      $\omega_d$$1.2 \times {10^{ - 3}}\;{\rm{Hz}}$
                      下載: 導出CSV
                      360彩票
                    • [1] 羅子人, 鐘敏, 邊星, 董鵬, 董玉輝, 高偉, 等. 地球重力場空間探測: 回顧與展望. 力學進展, 2014, 44(1): 291?337

                      1 Luo Zi-Ren, Zhong Min, Bian Xing, Dong Peng, Dong Yu-Hui, Gao Wei, et al. Mapping Earth's gravity in space: Review and future perspective. Advances in Mechanies, 2014, 44(1): 291?337
                      [2] Lange B. The Control and Use of Drag-Free Satellites[Ph. D. dissertation], Stanford University, 1964
                      [3] Dittus H, Lammerzahl C, Turyshev S. Lasers, Clocks, and Drag-Free: Exploration of Relativistic Gravity in Space. Berlin: Springer, 2008
                      [4] Haines R. Development of a drag-free control system. In: Proceedings of the 14th Annual AIAA/USU Conference on Small Satellites. Utah, USA: 2000
                      [5] Chapman P, Zentgraf, P, Jafry, Y. Drag-free control design including attitude transition for the STEP mission. In: Proceedings of the 5th ESA International Conference on Spacecraft Guidance, Navigation and Control Systems. 2003: 551−557
                      [6] 6 Canuto E, Massotti L. All-propulsion design of the drag-free and attitude control of the European satellite GOCE. Acta Astronautica, 2009, 64(2): 325?344
                      [7] Prieto D, Bona B. Orbit and attitude control for the European satellite GOCE. In: Proceedings of 2005 IEEE Networking, Sensing and Control. IEEE, 2005: 728−733
                      [8] 8 Wu S F, Fertin D. Spacecraft drag-free attitude control system design with Quantitative Feedback Theory. Acta Astronautica, 2008, 62(12): 668?682 doi:  10.1016/j.actaastro.2008.01.038
                      [9] Prieto D, Ahmad Z. A drag free control based on model predictive techniques. In: Proceedings of 2005 American Control Conference. Portland, USA: IEEE, 2005. 6: 1527−1532
                      [10] Fan H J, Zhang Y F, Zhao X. Finite-time control for LEO drag-free satellite with stochastic disturbance. In: Proceedings of the 27th Chinese Control and Decision Conference (2015 CCDC). Qingdao: 2015. 2091−2095
                      [11] Fleck M E, Starin S R. Evaluation of a drag-free control concept for missions in low earth orbit. In: Proceedings of AIAA Guidance, Navigation, and Control Conference and Exhibit. Austin, Texas: 11-14 August 2003
                      [12] 高志強. 自抗擾控制思想探究. 控制理論與應用, 2013, 30(12): 1498?1510 doi:  10.7641/CTA.2013.31087

                      12 Gao Zhi-Qiang. On the foundation of active disturbance rejection control. Control Theory & Applications, 2013, 30(12): 1498?1510 doi:  10.7641/CTA.2013.31087
                      [13] 韓京清. 從PID技術到"自抗擾控制"技術. 控制工程, 2002, 9(3): 13?18 doi:  10.3969/j.issn.1671-7848.2002.03.003

                      13 Han Jing-Qing. From PID technique to active disturbances rejection control technique. Control Engineering of China, 2002, 9(3): 13?18 doi:  10.3969/j.issn.1671-7848.2002.03.003
                      [14] 韓京清. 自抗擾控制技術: 估計補償不確定因素的控制技術. 北京: 國防工業出版社, 2008

                      Han Jing-Qing. Active Disturbance Rejection Control Technique: the Technique for Estimating and Compensating the Uncertainties. Beijing: National Defense Industry Press, 2008
                      [15] 李向陽, 哀薇, 田森平. 慣性串聯系統的自抗擾控制. 自動化學報, 2018, 44(3): 562?568

                      15 Li Xiang-Yang, Ai Wei, Tian Sen-Ping. Active disturbance rejection control of cascade inertia systems. Acta Automatica Sinica, 2018, 44(3): 562?568
                      [16] 金輝宇, 劉麗麗, 蘭維瑤. 二階系統線性自抗擾控制的穩定性條件. 自動化學報, 2018, 44(09): 1725?1728

                      16 Jin Hui-Yu, Liu Li-Li, Lan Wei-Yao. On stability condition of linear active disturbance rejection control for second-order systems. Acta Automatica Sinica, 2018, 44(09): 1725?1728
                      [17] 劉昊, 魏承, 譚春林, 劉永健, 趙陽. 空間充氣展開繩網系統捕獲目標自抗擾控制研究. 自動化學報, 2019, 45(09): 1691?1700

                      17 Liu Hao, Wei Cheng, Tan Chun-Lin, Liu Yong-Jian, Zhao Yang. Research on capturing target of space inflatable net capture system based on active disturbance rejection control. Acta Automatica Sinica, 2019, 45(09): 1691?1700
                      [18] 劉智勇, 何英姿. 相對位置和姿態動力學耦合航天器的自抗擾控制器設計. 航天控制, 2010, 28(2): 17?22

                      18 Liu Zhi-Yong, He Ying-Zi. An active disturbance rejection controller design for spacecraft with coupled relative position and attitude dynamics. Aerospace Control, 2010, 28(2): 17?22
                      [19] 吳忠, 黃麗雅, 魏孔明, 郭雷. 航天器姿態穩定自抗擾控制. 第三十二屆中國控制會議, 2013: 1031−1034

                      Wu Zhong, Huang Li-Ya, Wei Kong-Ming, Guo Lei. Active disturbance rejection control for spacecraft attitude. In: Proceedings of the 32nd Chinese Control Conference. IEEE, 2013: 1031−1034
                      [20] 吳忠, 黃麗雅, 魏孔明, 郭雷. 航天器姿態自抗擾控制. 控制理論與應用, 2013, 30(12): 1617?1622 doi:  10.7641/CTA.2013.31034

                      20 Wu Zhong, Huang Li-Ya, Wei Kong-Ming, Guo Lei. Active disturbance rejection control of attitude for spacecraft. Control Theory & Applications, 2013, 30(12): 1617?1622 doi:  10.7641/CTA.2013.31034
                      [21] Gao Z Q. Scaling and bandwidth-parameterization based controller tuning. In: Proceedings of the American control conference. 2006, 6: 4989-4996
                      [22] 22 Xue W C, Bai W Y, Yang S, Song K, Huang Y, Xie H. ADRC with adaptive extended state observer and its application to air-fuel ratio control in gasoline engines. IEEE Transactions on Industrial Electronics, 2015, 62(9): 1?10
                      [23] 23 Bai W Y, Xue W C, Huang Y, Fang H T. On extended state based Kalman filter design for a class of nonlinear time-varying uncertain systems. Science China Information Sciences, 2018, 61(4): 042201: 1?042201: 16
                      [24] Bai W Y, Xue W C, Huang Y, Fang H T. Extended state filter design for general nonlinear uncertain systems. In: Proceedings of the 54th Annual Conference of the Society of Instrument and Control Engineers of Japan (SICE). IEEE, 2015: 712−717
                      [25] Zhang X C, Xue W C, Fang H T, He X K. On extended state based Kalman-Bucy filter. In: Proceedings of the 7th Data Driven Control and Learning Systems Conference (DDCLS). IEEE, 2018: 1158−1163
                      [26] He X K, Zhang X C, Xue W C, Fang H T. Distributed Kalman filter for a class of nonlinear uncertain systems: An extended state method. In: Proceedings of the 21st International Conference on Information Fusion (FUSION). IEEE, 2018: 78−83
                      [27] Duan Z, Jilkov V P, Li X R. State estimation with quantized measurements: Approximate MMSE approach. In: Proceedings of International Conference on Information Fusion. IEEE, 2008: 1−6
                      [28] 28 Guo J, Zhao Y L, Sun C Y. State estimation with quantized innovations and communication channels. IET Control Theory & Applications, 2015, 9(17): 2606?2612
                      [29] Wang Y S. Research on control algorithm for the drag-free satellite[Ph. D. dissertation], Harbin Institute of Technology, 2011
                      [30] 謝永春, 雷擁軍, 郭建新. 航天器動力學與控制. 北京: 北京理工大學出版社. 2018

                      Xie Yong-Chun, Lei Yong-Jun, Guo Jian-Xin. Spacecraft Dynamics and Control. Beijing: Beijing Institute of Technology Press. 2018
                      [31] Simon D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches, John Wiley & Sons. 2006
                    • [1] 武憲青, 徐可心, 張益波. 基于輸出反饋的欠驅動TORA系統的有界輸入控制[J]. 自動化學報, doi: 10.16383/j.aas.c180625
                      [2] 劉昊, 魏承, 譚春林, 劉永健, 趙陽. 空間充氣展開繩網系統捕獲目標自抗擾控制研究[J]. 自動化學報, doi: 10.16383/j.aas.c180835
                      [3] 黃大榮, 柴彥沖, 趙玲, 孫國璽. 考慮多源不確定信息的路網交通擁堵狀態辨識方法[J]. 自動化學報, doi: 10.16383/j.aas.2018.c160373
                      [4] 李向陽, 哀薇, 田森平. 慣性串聯系統的自抗擾控制[J]. 自動化學報, doi: 10.16383/j.aas.2018.c160568
                      [5] 程赟, 陳增強, 孫明瑋, 孫青林. 多變量逆解耦自抗擾控制及其在精餾塔過程中的應用[J]. 自動化學報, doi: 10.16383/j.aas.2017.c170137
                      [6] 劉曉東. 針對一類非線性系統的多變量線性 擴張狀態觀測器及其收斂性分析[J]. 自動化學報, doi: 10.16383/j.aas.2016.c150707
                      [7] 李杰, 齊曉慧, 夏元清, 高志強. 線性/非線性自抗擾切換控制方法研究[J]. 自動化學報, doi: 10.16383/j.aas.2016.c150338
                      [8] 陳增強, 孫明瑋, 楊瑞光. 線性自抗擾控制器的穩定性研究[J]. 自動化學報, doi: 10.3724/SP.J.1004.2013.00574
                      [9] 李新德, 楊偉東, DEZERT Jean. 一種飛機圖像目標多特征信息融合識別方法[J]. 自動化學報, doi: 10.3724/SP.J.1004.2012.01298
                      [10] 甄子洋, 王志勝, 王道波. 基于信息融合估計的離散線性系統預見控制[J]. 自動化學報, doi: 10.1360/SP.J.1004.2010.00347
                      [11] 顧啟泰, 方靖. 廣義聯邦濾波器的全局最優性[J]. 自動化學報, doi: 10.3724/SP.J.1004.2009.01310
                      [12] 孫書利, 呂楠. 帶有色觀測噪聲多傳感器多重時滯系統分布式融合濾波器[J]. 自動化學報, doi: 10.3724/SP.J.1004.2009.00046
                      [13] 甄子洋, 王志勝, 王道波. 基于信息融合最優估計的非線性離散系統預測控制[J]. 自動化學報, doi: 10.3724/SP.J.1004.2008.00331
                      [14] 陳國棟, 賈培發. 基于擴張狀態觀測的機器人分散魯棒跟蹤控制[J]. 自動化學報, doi: 10.3724/SP.J.1004.2008.00828
                      [15] 喬國林, 童朝南, 孫一康. 基于神經網絡自抗擾控制的結晶器液位拉速協調系統研究[J]. 自動化學報, doi: 10.1360/aas-007-0641
                      [16] 潘泉, 于昕, 程詠梅, 張洪才. 信息融合理論的基本方法與進展[J]. 自動化學報
                      [17] 張啟忠, 蔣靜坪. 用于機器人信息融合的RS智能系統[J]. 自動化學報
                      [18] 韓崇昭, 朱洪艷. 多傳感信息融合與自動化[J]. 自動化學報
                      [19] 劉翔, 姜學智, 李東海, 李立勤, 胡雪蛟, 黎浩榮. 自抗擾控制器在過熱汽溫控制中應用的仿真研究[J]. 自動化學報
                      [20] 鄔永革, 楊靜宇. 基于融合信息的道路分割方法[J]. 自動化學報
                    • 加載中
                    計量
                    • 文章訪問數:  1804
                    • HTML全文瀏覽量:  677
                    • 被引次數: 0
                    出版歷程
                    • 收稿日期:  2019-07-05
                    • 錄用日期:  2019-12-08
                    • 網絡出版日期:  2020-01-17

                    飽和約束測量擴張狀態濾波與無拖曳衛星位姿自抗擾控制

                    doi: 10.16383/j.aas.c190515
                      基金項目:  國家重點研發計劃(2018YFA0703800), 國家自然科學基金項目(61773054, 61633003-3)資助
                      作者簡介:

                      北京科技大學碩士研究生. 主要從事自抗擾控制理論及應用等方面的研究.E-mail: changeandbebetter@163. com

                      北京控制工程研究所高級工程師. 主要從事航天器控制理論與控制工程等方面的研究.E-mail: sptan@amss.ac.cn

                      中國科學院數學與系統科學研究院副研究員. 主要從事非線性不確定系統控制與濾波、自抗擾控制等.E-mail: wenchaoxue@amss.ac.cn

                      北京科技大學自動化學院教授. 主要從事集值系統、信息物理系統的辨識與控制等方面的研究. 本文通信作者.E-mail: guojin@ustb.edu.cn

                      中國科學院數學與系統科學研究院研究員. 主要研究方向為系統建模與辨識、集值系統辨識與控制、金融系統建模與分析等.E-mail: ylzhao@amss.ac.cn

                    • 1如果\begin{document}$ {{a}}_k $\end{document}是一個向量, 那么用\begin{document}$ {{a}}_{k,i} $\end{document} 表示其第\begin{document}$ i $\end{document}個分量.2算法1與文[23]中的形式有所不同, 這主要是為了方便后面的信息融合算法設計.

                    摘要: 無拖曳衛星的本體姿態、衛星本體與測試質量間的相對位移及相對姿態的聯合控制受到外部擾動、輸入噪聲、測量噪聲及飽和約束、輸入耦合以及狀態耦合等因素的影響, 控制器的設計面臨挑戰. 本文采用基于擴張狀態的卡爾曼濾波對系統狀態和系統擾動進行實時估計, 引入自抗擾控制策略進行了控制器設計. 針對無拖曳控制子系統設計了測量飽和受限下的擴張狀態估計算法, 并進行了信息融合. 在設計控制律時不僅考慮了對外部擾動的補償, 還將系統狀態間的耦合關系看成內部擾動進行補償, 使得被控系統等價為“積分串聯型系統”, 在此基礎上實現了無拖曳衛星的聯合控制. 數值仿真驗證了方法的有效性和合理性.

                    1如果$ {{a}}_k $是一個向量, 那么用$ {{a}}_{k,i} $ 表示其第$ i $個分量.2算法1與文[23]中的形式有所不同, 這主要是為了方便后面的信息融合算法設計.

                    English Abstract

                    楊飛, 談樹萍, 薛文超, 郭金, 趙延龍. 飽和約束測量擴張狀態濾波與無拖曳衛星位姿自抗擾控制. 自動化學報, 2020, 45(x): 1?14. doi: 10.16383/j.aas.c190515
                    引用本文: 楊飛, 談樹萍, 薛文超, 郭金, 趙延龍. 飽和約束測量擴張狀態濾波與無拖曳衛星位姿自抗擾控制. 自動化學報, 2020, 45(x): 1?14. doi: 10.16383/j.aas.c190515
                    Yang Fei, Tan Shu-Ping, Xue Wen-Chao, Guo Jin, Zhao Yan-Long. Extended state filtering with saturation-constrainted observations and active disturbance rejection control of position and attitude for drag-free satellites. Acta Automatica Sinica, 2020, 45(x): 1?14. doi: 10.16383/j.aas.c190515
                    Citation: Yang Fei, Tan Shu-Ping, Xue Wen-Chao, Guo Jin, Zhao Yan-Long. Extended state filtering with saturation-constrainted observations and active disturbance rejection control of position and attitude for drag-free satellites. Acta Automatica Sinica, 2020, 45(x): 1?14. doi: 10.16383/j.aas.c190515
                    • 地球重力場及其時變信息反映著地球內部物質結構的變化, 對地球重力場的研究意義重大[1]. 利用衛星技術進行地球重力場探測不受地形等自然條件的影響, 為解決全球高覆蓋率、高精度、高空間分辨率和高時間重復率重力測量開辟了新的有效途徑. 無拖曳控制技術是重力梯度測量衛星的關鍵技術之一, 其目的是為了補償在軌衛星受到的干擾力及力矩, 使得衛星在地球重力場的作用下運行, 即運行在純引力軌道上. 對于低軌衛星而言,其受到的主要干擾為大氣阻力或力矩(Drag Force or Torque), 因此國外學者將這種通過控制器抵消大氣阻力或力矩的衛星稱之為無拖曳衛星(Drag-Free Satellite), 其控制系統稱之為無拖曳控制系統(Drag-Free Control System)[2,3].

                      PID作為一種經典控制方法, 被很多學者應用到了無拖曳衛星控制中[4], 這種設計方式簡單穩定、魯棒性較高, 但難以消除外部擾動的干擾. 針對GP-B (Gravity Probe B)衛星運行于加速度計模式的情況, 文[5]對 質量塊懸浮控制系統進行了設計, 采用了自適應線性二次型調節器算法, 并采用PD控制算法作為備份. 文[6]將內嵌模型控制理論用于了 GOCE (Gravity field and steady-state Ocean Circulation Explorer) 衛星的無拖曳和姿態控制的設計之中. 在無拖曳控制系統的設計中非常重要的一個問題就是魯棒性, 很多學者采用$ H\infty $的方法來解決這一問題. 文[7]應用線性矩陣不等式多目標優化方法設計了$ H\infty $控制器, 同時在設計中考慮了諸多約束條件, 以保證GOCE衛星無拖曳控制任務的實現. 定量反饋理論(QFT)是一種基于頻域 的魯棒控制設計理論, 可用于具有高度不確定性的系統控制器設計, 文[8]利用QFT對LISA (Laser Interferometer Space Antenna)衛星的無拖曳和姿態控制系統進行了設計. 文[9]將模型預測技術用于處理無拖曳衛星控制的約束問題. 文[10]運用有限時間Lyapunov穩定性理論設計了無拖曳衛星控制系統的有限時間控制器.

                      盡管眾多控制方法都用于了無拖曳控制, 但實際中真正實現高精度或者極低“剩余”加速 度的無拖曳控制還面臨諸多理論和技術上的困難. 首先, 至今沒有建立準確的大氣模型, 根據CHAMP (Challenging Minisatellite Payload)數據, 大氣變化很有可能比以往所建模型都要劇烈[11]. 其次, 重力梯度儀的測量精度很高, 但量程相對較小, 非保守力帶來的加速度很容易超出重力梯度儀的測量量程, 即測量信息是飽和約束的限制. 此外,還有諸如外部擾動、輸入耦合以及狀態耦合等各方面的影響.

                      綜合考慮到這些因素, 本文將自抗擾控制技術用于無拖曳衛星的位姿聯合控制. 自抗擾控制起源于韓京清先生對控制理論的反思[12,13], 后來經過不斷的探索和完善而形成了一套系統的控制理論[14-17]. 由于自抗擾控制對控制對象的參數和結構變化具有較好的魯棒性和自適應能力等特點, 使得其在航天領域逐步引起重視. 針對慢旋非合作目標在軌服務任務, [18]設計了姿態和軌道耦合系統的自抗擾控制器, 仿真結果表明, 自抗擾控制器在解決存在非線性和耦合特性的相對位置和姿態耦合系統的控制問題上能夠取得不錯的控制效果. 為抑制航天器自身結構參數變化和內外擾動對姿態控制精度和姿態穩定度的影響, [19, 20]對航天器姿態穩定模式采用自抗擾控制技術設計了姿態控制器, 在仿真實驗中取得了良好效果.

                      作為自抗擾控制關鍵環節的擴張狀態觀測器設計近些年也受到了廣泛關注. [21]引入了帶寬的概念將非線性擴張狀態觀測器進行線性化和參數化以簡化設計工作. [22]利用系統的狀態和外界環境(如噪聲)為觀測器的增益設計自適應調節律, 實現了對觀測器增益的實時調整. 針對具有不確定性動態的非線性時變系統, [23]和[24]設計了基于擴張狀態的Kalman濾波器, 在估計系統狀態的同時實現了對系統不確定動態的估計. [25]結合了Kalman-Bucy濾波器和擴張狀態觀測器的優勢, 設計出了基于擴張狀態的Kalman-Bucy濾波器, 并嚴格證明了它的穩定性. 針對具有非線性不確定動力學的離散時變隨機系統, [26]研究了其在時變拓撲傳感器網絡下的分布式狀態估計問題.

                      自抗擾控制技術在對付模型不確定性和抗干擾方面為我們研究無拖曳控制提供了新的思路, 但作為其關鍵步驟的擴張狀態觀測器設計還要面臨(加速度計的)測量信息飽和受限的問題. 實際上, 飽和受限帶來的主要困難在于量程范圍外的信息只能是粗糙的集值信息. 借鑒近些年來發展的集值濾波技術[27,28]和擴張狀態估計技術[14,23], 本文設計了面向無拖曳控制子系統的測量飽和受限擴張狀態濾波算法, 并結合擴張狀態卡爾曼濾波進行了信息融合. 在設計控制律時不僅考慮了對外部擾動的補償, 還將系統狀態間的耦合關系看成內部擾動進行補償, 使得被控系統等價為“積分串聯型系統”, 在此基礎上實現了無拖曳衛星的聯合控制.

                      本文的主要創新和貢獻可總結為: 1) 不同于經典的精確測量(或帶有一定的加性噪聲)下的狀態估計, 本文在測量飽和受限的情形設計了遞推的擴張狀態濾波算法, 克服了約束區間外的集值信息帶來的高度非線性困難; 2) 利用了信息融合技術, 解決了利用加速度計測量信息對系統狀態(位移和速度)進行估計時存在的累積誤差問題; 3) 引入了自抗擾控制的思想, 實現了無拖曳衛星的位姿聯合控制器設計, 成功處理了三種類型的耦合: 子系統間的耦合、控制輸入的耦合、子系統內部各個狀態的耦合.

                      接下來內容的結構如下: 第二節介紹無拖曳衛星控制系統模型. 第三節介紹自抗擾控制器結構, 推導集值擴張狀態濾波算法, 并進行信息融合, 進而設計了位姿聯合控制律. 第四節給出了仿真實驗. 第五節對本文進行了總結, 并對接下來需要進行的研究工作進行了簡要介紹.

                      • 無拖曳衛星的工作原理(見圖1)是在衛星密封腔內放置一個測試質量, 衛星包圍著測試質量而不與之發生接觸. 位于密封腔內的測試質量處于完全保守力環境, 而衛星受到外界擾動影響會相對于測試質量產生位移, 此時控制系統通過敏感器測量此位移, 并使用推力器對衛星平臺進行補償, 最終實現衛星平臺與測試質量同步運動, 實現“無拖曳”狀態.

                        圖  1  無拖曳衛星控制系統圖

                        Figure 1.  Diagram of drag-free satellite control system

                        通常依據控制目標, 整個無拖曳衛星系統被劃分為三個控制回路: 衛星本體姿態控制回路, 負責控制衛星本體姿態; 衛星本體與測試質量相對位移回路, 負責控制衛星本體與測試質量間的相對距離; 衛星本體與測試質量相對姿態控制回路, 負責調整衛星本體與測試質量間的相對姿態.

                        無拖曳衛星控制系統的動力學簡化模型[29]如下:

                        $$ {{\ddot {{\varphi}} }_{{\rm{sc}}}} = I_{{\rm{sc}}}^{ - 1}({{{T}}_{Csc}} + {{{w}}_{{T_{Csc}}}} + {{{T}}_{Dsc}}), \qquad\qquad\quad $$ (1)
                        $$ \begin{split} {{\ddot {{r}}}_{{\rm{rel}}}} =& - \frac{{{K_{{\rm{trans}}}}}}{{{m_{{\rm{tm}}}}}}{{{r}}_{{\rm{rel}}}} - \frac{{{D_{{\rm{trans}}}}}}{{{m_{{\rm{tm}}}}}}{{\dot {{r}}}_{{\rm{rel}}}} -\qquad\;\\ &\frac{\mathbb{I}_{3}}{{{m_{{\rm{sc}}}}}}({{{F}}_{Csc}} + {{{w}}_{{F_{Csc}}}} + {{{F}}_{Dsc}}), \end{split}\qquad\qquad$$ (2)
                        $$ \begin{split} {{\ddot {{\varphi}} }_{{\rm{rel}}}} =& {{\ddot {{\varphi}} }_{{\rm{tm}}}} - {{\ddot {{\varphi}} }_{{\rm{sc}}}} = I_{{\rm{tm}}}^{ - 1}{K_{{\rm{rot}}}}{{{\varphi}} _{{\rm{rel}}}} + I_{{\rm{tm}}}^{ - 1}{D_{{\rm{rot}}}}{{\dot {{\varphi}} }_{{\rm{rel}}}} +\\ & I_{{\rm{tm}}}^{ - 1}({{{T}}_{Ctm}} + {{{w}}_{{T_{Ctm}}}} + {{{T}}_{Dtm}})-\\ &I_{{\rm{sc}}}^{ - 1}({{{T}}_{Csc}} + {{{w}}_{{T_{Csc}}}} + {{{T}}_{Dsc}}), \end{split}$$ (3)

                        其中$ {{{{\varphi}} _{{\rm{sc}}}}} $表示衛星本體的姿態角, $ {{{{T}}_{Csc}}} $表示作用在衛星本體上的控制力矩, $ {{{w}}_{{T_{Csc}}}} $表示與$ {{{{T}}_{Csc}}} $對應的輸入噪聲, $ {{{{T}}_{Dsc}}} $表示作用在衛星本體上的擾動力矩, $ {{{{r}}_{{\rm{rel}}}}} $表示衛星本體和測試質量間的相對位移, $ {{{{F}}_{Csc}}} $表示作用在衛星本體上的控制力, $ {{{w}}_{{F_{Csc}}}} $表示與$ {{{{F}}_{Csc}}} $對應的輸入噪聲, $ {{{{F}}_{Dsc}}} $表示作用在衛星本體上的擾動力, $ {{{{\varphi}} _{{\rm{rel}}}}} $表示衛星本體和測試質量間的相對姿態角, $ {{{{\varphi}} _{{\rm{tm}}}}} $表示測試質量的姿態角, $ {{{{T}}_{Ctm}}} $表示作用在測試質量上的控制力矩, $ {{{w}}_{{T_{Ctm}}}} $表示與$ {{{{T}}_{Ctm}}} $對應的輸入噪聲, $ {{{{T}}_{Dtm}}} $表示作用在測試質量上的擾動力矩, 這些都是在$ X,Y,Z $方向或俯仰(pitch), 偏航(yaw), 滾動(roll)方向上有分量的三維向量, 比如 $ {{{\varphi}} _{{\rm{sc}}}} = {[ {\begin{array}{*{20}{c}} {{{\varphi} _{{\rm{sc,pitch}}}}}&{{\varphi _{{\rm{sc,yaw}}}}}&{{\varphi _{{\rm{sc,roll}}}}} \end{array}} ]^{\rm T}} $; $ {I_{{\rm{sc}}}} $代表衛星本體的轉動慣量, $ {{{K_{{\rm{trans}}}}}} $、$ {{{D_{{\rm{trans}}}}}} $表示衛星與測試質量間的平動耦合系數, $ {I_{{\rm{tm}}}} $代表測試質量的轉動慣量, $ {{{K_{{\rm{rot}}}}}} $、$ {{{D_{{\rm{rot}}}}}} $表示衛星與測試質量間的旋轉耦合系數, $ I_3 $代表單位矩陣, 它們都是$ 3\times 3 $的矩陣; $ {{{m_{{\rm{tm}}}}}} $代表測試質量的質量, $ {{{m_{{\rm{sc}}}}}} $代表衛星本體的質量.

                        本文旨在將自抗擾控制策略應用到無拖曳衛星的位姿控制中, 以期能克服干擾、噪聲及耦合等因素的綜合影響并取得良好的控制效果.

                      • 自抗擾控制器的結構如圖2所示, 主要由跟蹤微分器(TD)、擴張狀態觀測器(ESO)、非線性組合PID (NLPID) 以及擾動補償等四部分構成. 跟蹤微分器會根據控制目標和對象的承受能力安排合適的過渡過程; 擴張狀態觀測器用于估計系統所受的擾動; 非線性組合PID的設計可以使得在誤差較小時產生相對較大的控制增益, 誤差較大時產生相對較小的控制增益, 以求提升控制效果; 擾動補償模塊可以根據擴張狀態觀測器估計出的系統受到的擾動量來對系統進行補償.

                        圖  2  自抗擾控制器框圖

                        Figure 2.  Structure of active disturbance rejection controller

                      • 自抗擾控制的核心是對擾動進行有效補償, 關鍵一步便是通過構建擴張狀態觀測器來主動的從系統的輸入、輸出信息中提取出擾動信息. 若令$ {{X}}_o = {[ {\begin{array}{*{20}{c}} {{{\varphi}} _{{\rm{sc}}}^{\rm T}}\!&\!{\dot {{\varphi}} _{{\rm{sc}}}^{\rm T}}&{{{r}}_{{\rm{rel}}}^{\rm T}}\!&\!{\dot {{r}}_{{\rm{rel}}}^{\rm T}}&{{{\varphi}} _{{\rm{rel}}}^{\rm T}}\!&\!{\dot {{\varphi}} _{{\rm{rel}}}^{\rm T}} \end{array}}]^{\rm T}} $${{Y}} =[ {{{\varphi}} _{sc}^{\rm T}} $$ {{\begin{array}{*{20}{c}}{{{r}}_{{\rm{rel}}}^{\rm T}}&{{{\varphi}}_{{\rm{rel}}}^{\rm T}} \end{array}}]^{\rm T}} $, 則系統(1)-(3)可以寫成如下形式[29]:

                        $$ {\dot {{X}}_o} = A_{o}{{X}}_o + B_o({{u}} + {{w}} + {{f}}), $$ (4)
                        $$ {{Y}} = C_o{{{X}}_o} + {{d}}, $$ (5)

                        其中$ A_o = $

                        $$ \left[ {\begin{array}{*{20}{c}} {{0_3}}&{{\mathbb{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{\mathbb{I}_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{ - \dfrac{{{K_{{\rm{trans}}}}}}{{{m_{tm}}}}}&{ - \dfrac{{{D_{{\rm{trans}}}}}}{{{m_{tm}}}}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{\mathbb{I}_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{I_{tm}^{ - 1}{K_{{\rm{rot}}}}}&{I_{tm}^{ - 1}{D_{{\rm{rot}}}}} \end{array}} \right], $$
                        $$ B_o = \left[ {\begin{array}{*{20}{c}} {{0_3}}&{{0_3}}&{{0_3}}\\ {I_{sc}^{ - 1}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{ - \dfrac{{\mathbb{I}_3}}{{{m_{sc}}}}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}\\ { - I_{{\rm{sc}}}^{ - 1}}&{{0_3}}&{I_{tm}^{ - 1}} \end{array}}\right], \qquad\qquad\qquad\qquad$$
                        $$ C_o = \left[ {\begin{array}{*{20}{c}} {{\mathbb{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{\mathbb{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{\mathbb{I}_3}}&{{0_3}} \end{array}}\right], \qquad\qquad\qquad$$

                        $ {{u}} = [{{T}}_{Csc}\; {{F}}_{Csc}\; {{T}}_{Ctm}]^{\rm T} $ 為期望的系統輸入, $ {{w}} = [{{w}}_{{T_{Csc}}}\; {{w}}_{{F_{Csc}}}\; {{w}}_{{T_{Ctm}}}]^{\rm T} $為輸入噪聲, ${{f}} = [{{T}}_{Dsc}\; $$ {{F}}_{Dsc}\; {{T}}_{Dtm}]^{\rm T} $為系統受到的外部擾動, 其標稱模型$ \bar{{{f}}} $已知; $ {{d}} $為輸出測量噪聲. 這里及以后, $ \mathbb{I}_{3} $表示$ 3\times 3 $的單位陣, $ 0_{m} $表示元素全為0的$ m\times m $矩陣.

                        注 2.1在系統(1)-(3)中, $ {I_{{\rm{sc}}}},{I_{{\rm{tm}}}} $等參數也可能具有一定的不確定性, 此時也可以轉化成(4)的形式進行處理. 當(1)中的慣量矩陣具有不確定時, 有: $ ({I_{{\rm{sc}}}} \!+\! \Delta {I_{{\rm{sc}}}}){{\ddot {{\varphi}} }_{{\rm{sc}}}} = ({{{T}}_{Csc}} + {{{w}}_{{T_{Csc}}}} + {{{T}}_{Dsc}}). $${{T}}_{Dsc}' = $$ {{{T}}_{Dsc}} - \Delta {I_{{\rm{sc}}}}{{\ddot {{\varphi}} }_{{\rm{sc}}}} $, 則該式可以轉化為$ {{\ddot {{\varphi}} }_{{\rm{sc}}}} = I_{{\rm{sc}}}^{ - 1}({{{T}}_{Csc}} + $$ {{{w}}_{{T_{Csc}}}} + {{T}}_{Dsc}') $. 類似地, (2)和(3)中的參數不確定性也可以進行這樣的轉化.

                        $ {{f}} $作為擴張狀態, 取$ {{X}} = {[{{\varphi}} _{{\rm{sc}}}^{\rm T}{\rm{\; \; }}\dot {{\varphi}} _{{\rm{sc}}}^{\rm T}{\rm{\; \; }}{{T}}_{Dsc}^{\rm T}{\rm{\; \; }}{{r}}_{{\rm{rel}}}^{\rm T}{\rm{\; \; }}} $ $ {\dot {{r}}_{{\rm{rel}}}^{\rm T}{\rm{\; \; }}{{F}}_{Dsc}^{\rm T}{\rm{\; \; }}{{\varphi}} _{{\rm{rel}}}^{\rm T}{\rm{\; \; }}\dot {{\varphi}} _{{\rm{rel}}}^{\rm T}{\rm{\; \; }}{{T}}_{Dtm}^{\rm T}]^{\rm T}} $, 則在(4)-(5)的基礎上可以構建系統 的擴張狀態方程:

                        $$ \left\{ \begin{array}{l} \dot {{X}} = \; \; A{{X}} + B({{u}} + {{w}}) + {B_e}\dot {{f}},\\ {{Y}} = \; \; C{{X}} + {{d}}, \end{array}\right. $$ (6)

                        其中$ B_e $是將系統變形為擴張系統時的關于系統擾動項的系數矩陣, 即有

                        $$ {B_e} = {\left[ {\begin{array}{*{20}{c}} {{0_3}}&{{0_3}}&{{\mathbb{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{\mathbb{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{\mathbb{I}_3}} \end{array}} \right]^{\rm T}}. $$

                        對于擴張系統(6)可以通過構建(7)形式的擴張狀態觀測器實現對系統狀態的跟蹤:

                        $$ {\dot {\widehat{{{X}}}}} = A{\widehat{{{X}}}} + B{{u}} + L({{Y}} - C{\widehat{{{X}}}}). $$ (7)

                        為了便于工程應用, [21]中提出了一種帶寬參數的方法來設計擴張狀態觀測器, 即選定$ L $使(7)滿足:

                        $$ {\rm {eig}}(A - LC) = {(s + {\omega _o})^n}, $$

                        其中$ {\rm {eig}}(\cdot) $是特征多項式計算符號, 即$ {\rm {eig}}(A - LC) $表示矩陣$ A - LC $的特征多項式; $ {\omega _o} $是設定的觀測器帶寬, $ n $為擴張系統的維數.

                        考慮到對系統的測量是離散進行的, 而且系統輸入輸出往往帶有噪聲. 對于這種情況下的擴張狀態觀測器設計問題, [23]設計了一種基于擴張狀態的卡爾曼濾波器(ESKF), 依據最小均方差原理對系統(7)中的$ L $進行實時調整, 實現了擾動估計以及濾波功能間的均衡. 將(6)寫為如下離散形式

                        $$ \left\{ \begin{array}{l} {{{X}}_{k+1}} = \; \; {A_d}{{{X}}_{k}} + {B_d}({{{u}}_{k}} + {{{w}}_{k}}) + {B_e}{{{G}}_{k}},\\ {{{Y}}_k} = \; \; {C_d}{{{X}}_k} + {{{d}}_k}, \end{array}\right. $$ (8)

                        其中$ {{{G}}_{k - 1}} = {{{f}}_k} - {{{f}}_{k - 1}} $.

                        如果系統(8)滿足以下四個假設, 則可進行算法1形式的遞推實現對系統擴張狀態的估計:

                        假設一: ($ {A_d},{C_d} $)可觀測.

                        假設二: $ \{ {{{w}}_k}\} _0^\infty $, $ \{ {{{d}}_{k}}\} _0^\infty $為互不相關的零均值高斯噪聲, 且有$ {\rm E}({{{w}}_k}{{w}}_k^{\rm T}) \le {S_k} $, $ {\rm E}({{{d}}_{k}}{{{d}}_{k}^{\rm T}}) \le {R_{k}} $.

                        假設三: $ {\rm E}( {[ {{{{X}}_0} - {{\widehat {{X}}}_0}} ]{{[ {{{{X}}_0} -{{\widehat {{X}}}_0}}]^{\rm T}}}} ) \le {P_0} $, 其中$ {\widehat {{X}}}_0 $是對$ {{{{X}}_0}} $的估計值, $ {P_0} $為已知的常數矩陣.

                        假設四: $ {\rm E}\left( {{{G}}_{k,i}^2} \right) \le {{{q}}_{k,i}}, i = 1,2,\cdots,l, \forall k \ge 0 $, 其中$ l $表示$ {{{G}}_{k}} $的維數, $ {{{q}}_{k,i}} $已知有界.

                        算法1: 基于擴張狀態的卡爾曼濾波[23]

                        初始化:

                        給定初值$ {{ {\hat{X}}}_0},{P_0} $.

                        時間更新:

                        $$ \begin{split} {\hat{X}}_k^- =&{A_d}{{ {\hat{X}}}_{k - 1}} + {B_d}{{{u}}_{k - 1}} + {B_e}{{ {\hat{G}}}_{k - 1}},\\ &P_k^- = (1 + {\theta }){A_d}{P_{k - 1}}{A_d}^{\rm T} +\\ &(1 + \frac{1}{{{\theta }}}){Q_{1,k - 1}} + {Q_{2,k - 1}}, \end{split} $$

                        其中$ \theta \!=\! \sqrt {\dfrac{{{\rm{trace}}({Q_{1,0}})}}{{{\rm{trace}}({P_0})}}} $, $ {Q_{1,k-1}} \!=\! 4{B_e}{Q_{k-1}}B_e^{\rm T} $, ${Q_{k-1}} = $$ l{\rm{diag}}({{{q}}_{k\!-\!1,1}},{{{q}}_{k\!-\!1,2}}, \cdots ,{{{q}}_{k\!-\!1,l}}) $, $ {Q_{2,k-1}} \!=\! {B_d}{S_{k-1}}B_d^{\rm T} ,$ $ {{ {\hat{G}}}_{k-1}} $表示對$ {{{G}}_{k-1}} $的估計值, 取值如下

                        $$ \begin{array}{l} {{ {\hat{G}}}_{k-1,i}} \\ = \left\{ \begin{array}{l} \Delta {{ {\hat{f}}}_{k-1,i}}, \;\;\;\; -\sqrt {{{{q}}_{k-1,i}}}\leq\Delta {{ {\hat{f}}}_{k-1,i}}\leq\sqrt {{{{q}}_{k-1,i}}}; \\ -\sqrt {{{{q}}_{k-1,i}}}, \;\;\;\; \Delta {{ {\hat{f}}}_{k-1,i}}<-\sqrt {{{{q}}_{k-1,i}}}; \\ \sqrt {{{{q}}_{k-1,i}}}, \;\;\;\; \Delta {{ {\hat{f}}}_{k-1,i}}>\sqrt {{{{q}}_{k-1,i}}}, \\ \end{array} \right. \end{array} $$ (9)

                        其中$ \Delta { {\hat{f}}_{k-1,i}} = {\bar {{f}}_{k,i}} - {\bar {{f}}_{k-1,i}} $.

                        測量更新:

                        $$ \begin{split} {\hat{X}}_k =& {\hat{X}}_k^ - + {K_k}({{{Y}}_k} - {C_d} {\hat{X}}_k^ -),\\ P_k =& (\mathbb{I} - {K_k}{C_d})P_k^-{(\mathbb{I} - {K_k}{C_d})^{\rm T}}+{K_k}R_kK_k^{\rm T},\\ K_k = &P_k^-{C_d^{\rm T}}{(C_d{P_k^-}{C_d^{\rm T}} + {R_k})^{ - 1}}. \end{split} $$

                        上述算法中, $ \mathbb{I} $表示適當維數的單位矩陣; $ {\rm {trace}}(\cdot) $表示矩陣的跡(主對角線上各個元素的總和); $ {\rm{diag}}({a_1},\cdots ,{a_n}) $表示對角線元素為$ a_1,\cdots,a_n $的對角矩陣.

                        注 2.2 式(9)中的$ {\hat{G}}_{k-1,i} $$ {\hat{G}}_{k-1} $的第$ i $分量, 它的取值與$ \bar{{{f}}} $的差分密切相關. $ \bar{{{f}}} $是對$ {{f}} $先驗知識的建模, 而$ {{f}} $是衛星受到的外部擾動力/力矩, 其一般與系統狀態和時間相關, 所以$ \bar{{{f}}} $往往是系統狀態$ {{X}}(t) $和時間$ t $的函數, 它的具體表達式的精確程度依賴于我們先驗知識的豐富程度, 一般難以統一給出. 此外, 從算法1的形式上可以看出, $ {\hat{G}} $的估計效果會直接影響到濾波算法的估計精度, 進而影響到閉環系統的控制效果.

                      • 無拖曳控制子系統是整個梯度測量衛星的核心部分, 除了(5)中提供的測量外, 還配備有高精度的加速度計, 可以實現對衛星本體的殘余加速度的精確測量. 但加速度計的測量范圍相對較小, 非保守力帶來的加速度很容易超出測量量程, 即測 量信息受飽和約束的限制[30]. 下面首先給出飽和約束下的擴張狀態濾波算法, 然后和算法1中的估計結果進行融合, 最終完成無拖曳控制子系統的擴張狀態估計.

                      • 為方便介紹算法的設計思路, 這里考慮某一方向上的無拖曳控制系統(2), 把其寫成狀態空間模型:

                        $$ {{\dot {{X}}}_o} = {A_o}{{{X}}_o} + {{{B}}_o}({ u} + { w} + { f}), $$ (10)

                        其中$ {{{{X}}}_o} = {[ {\begin{array}{*{20}{c}}{{r}_{{\rm{rel}}}}&{{\dot r}_{{\rm{rel}}}} \end{array}}]^T} $, $ u $為期望的系統輸入, $ w $為輸入噪聲, $ f $為系統受到的外部擾動, $ d $為測量噪聲. 注意到殘余加速度

                        $$ \begin{split} a =& \frac{{{K_{{\rm{trans}}}}}}{{{m_{{\rm{tm}}}}}}{r_{{\rm{rel}}}} + \frac{{{D_{{\rm{trans}}}}}}{{{m_{{\rm{tm}}}}}}{\dot r_{{\rm{rel}}}} +\\ &\frac{{{\mathbb{I}_3}}}{{{m_{{\rm{sc}}}}}}({F_{Csc}} + {w_{{F_{Csc}}}} + {F_{Dsc}}), \end{split} $$

                        所以對殘余加速度的測量方程為

                        $$ y = {{{C}}_o}{{{X}}_o} + D_o(u+w+f)+d. $$ (11)

                        $ {{X}} = {[{\begin{array}{*{20}{c}} {{r}_{{\rm{rel}}}}&{{\dot r}_{{\rm{rel}}}}&f \end{array}}]^{\rm T}} $, 根據(10)-(11)可以構建系統的擴張狀態方程:

                        $$ \left\{ \begin{array}{l} \dot {{X}} = \; \; A{{X}} + {{B}}(u + w) + {{{B}}_e}\dot f,\\ y = \; \; {{C}}{{X}} + D(u+w)+d, \end{array}\right. $$ (12)

                        其中$ A = \left[ {\begin{array}{*{20}{c}} {{A_o}}&{{{{B}}_o}}\\ 0&0 \end{array}} \right] $, $ {{B}} = \left[ {\begin{array}{*{20}{c}} {{{{B}}_o}}\\ 0 \end{array}} \right] $, $ {{{B}}_e} = [0\; \; 0\; \; 1]^{\rm T} $, $ {{C}} = [ {\begin{array}{*{20}{c}} {{{{C}}_o}}&D_o \end{array}}] $, $ D = D_o $.

                        設采樣間隔為$ h $, 將(12)離散化得到:

                        $$ \left\{ \begin{array}{l} {{{X}}_{k + 1}} = \; \; {A_d}{{{X}}_k} + {{{B}}_d}{u_k} + {{{B}}_d}{w_k} + {{{B}}_e}{G_k},\\ {y_{k}} = \; \; {{{C}}}{{{X}}_k} + D(u_k+w_k) + {d_{k}}, \end{array}\right. $$ (13)

                        其中 $ {G_k} = {f_{k{{ + }}1}} - {f_{k}} $.

                        記飽和約束下的測量信息$ {y_{c,k}} $為(如圖3所示):

                        圖  3  飽和約束測量

                        Figure 3.  Saturation-constrainted observations

                        $$ {y_{c,k}} = {\rm{sat}}({y_{k}},\underline y,\bar y) = \left\{ \begin{array}{l} \underline y,\; \; {y_{k}} \le \underline y;\\ {y_k},\; \; \underline y < {y_{k}} < \bar y;\\ \bar y,\; \; {y_{k}} \ge \bar y, \end{array} \right. $$ (14)

                        其中$ \underline y $為測量范圍下限, $ \bar y $為測量范圍上限.

                        定義: $ Y_c^k = \{ {y_{c,1}},{y_{c,2}}, \cdots, {y_{c,k}}\}, $ 即用$ {Y_c^k} $表示傳感器在$ k $次測量中得到的所有信息. 在3.1節的假設一、二、三、四下, 系統的時間更新可以寫為:

                        $$ \begin{split} {\hat{X}}_k^ - =& {\rm E}[{{X}}_k|Y_c^{k-1}]= \\ &{A_d}{{ {\hat{X}}}_{k - 1}} + {{{B}}_d}{u_{k - 1}} + {{{B}}_e}{{\hat G}_{k - 1}}, \end{split} $$

                        其中$ { {\hat{X}}}_{k - 1} = {\rm E}[{{X}}_{k-1}|Y_c^{k-1}] $, 其遞推關系由(31)和(33)給出; $ {\hat G}_{k - 1} = {\rm E}[G_{k-1}|Y_c^{k-1}] $, 由于$ f $未知, 故其表達式難以精確給出, 類似于算法1, 由(9)近似給出.

                        相應的預測誤差為:

                        $$ \begin{split} {{e}}_k^ - = &{{X}}_k - {\hat{X}}_k^ - = \\ & {A_d}{{{e}}_{k - 1}} + {{{B}}_d}{w_{k - 1}} + {{{B}}_e}{{\tilde G}_{k - 1}}, \end{split} $$

                        其中$ {{e}}_{k-1} = {{X}}_{k-1}-{ {\hat{X}}}_{k - 1} $; $ {{\tilde G}_{k - 1}} = {G_{k - 1}} - {{\hat G}_{k - 1}} $. 于是, 可以得到

                        $$ \begin{split} P_k^- \; = & {\rm E}[{{e}}_k^ - {({{e}}_k^ - )^{\rm T}}|Y_c^{k-1}] = \\ &{A_d}{\rm E}[{{{e}}_{k - 1}}{{e}}_{k - 1}^{\rm T}|Y_c^{k-1}]{A_d}^{\rm T}+\\ &{{{B}}_d}{\rm E}[w_{k - 1}^2|Y_c^{k-1}]{{B}}_d^{\rm T}+\\ &{{{B}}_e}{\rm E}[\tilde G_{k - 1}^2|Y_c^{k-1}]{{B}}_e^{\rm T}+\\ &{\rm E}[{A_d}{{{e}}_{k - 1}}\tilde G_{k - 1}{{B}}_e^{\rm T}|Y_c^{k-1}] +\\ &{\rm E}[{{{B}}_e}{{\tilde G}_{k - 1}}{{e}}_{k - 1}^{\rm T}{A_d}^{\rm T}|Y_c^{k-1}]. \end{split} $$ (15)

                        根據楊氏不等式, 對$ \forall \theta > 0 $有:

                        $$ \begin{split} &{\rm E}[{A_d}{{{e}}_{k - 1}}\tilde G_{k - 1}{{B}}_e^{\rm T}|Y_c^{k-1}] +\\ &\quad {\rm E}[{{{B}}_e}{{\tilde G}_{k - 1}}{{e}}_{k - 1}^{\rm T}{A_d}^{\rm T}|Y_c^{k-1}]\le \\ &\quad\theta {A_d}{\rm E}[{{{e}}_{k - 1}}{{e}}_{k - 1}^{\rm T}|Y_c^{k-1}]{A_d}^{\rm T} +\\ &\quad\dfrac{1}{\theta }{{{B}}_e}{\rm E}[\tilde G_{k - 1}^2|Y_c^{k-1}]{{B}}_e^{\rm T}. \end{split} $$ (16)

                        在上式中, 等式成立當且僅當$\theta {A_d}{{{e}}_{k - 1}} = $$ {{{B}}_e}{{\tilde G}_{k - 1}} $. 又因為$ {\rm E}[\tilde G_{k - 1}^2|Y_c^{k-1}] \le 4{q_{k - 1}} $, 所以

                        $$ {{{B}}_e}{\rm E}[\tilde G_{k - 1}^2|Y_c^{k-1}]{{B}}_e^{\rm T} \le 4{{{B}}_e}{q_{k - 1}}{{B}}_e^{\rm T}. $$ (17)

                        注意到$ {\rm E}[w_{k - 1}^2|Y_c^{k-1}]\leq S_{k-1} $并將(16)和(17)代入(15)可得: 對$ \forall \theta > 0 $, 有

                        $$ \begin{split} P_k^- \; \le & {A_d}{P_{k - 1}}{A_d}^{\rm T} + {Q_{1,k - 1}} + {Q_{2,k - 1}} +\\ &\theta {A_d}{P_{k - 1}}{A_d}^{\rm T} + \frac{1}{\theta }{Q_{1,k - 1}}, \end{split} $$

                        其中

                        $$ {Q_{1,k - 1}} = 4{B_e}{q_{k - 1}}B_e^{\rm T}, $$ (18)
                        $$ {Q_{2,k - 1}} = {B_d}S_{k-1}B_d^{\rm T}; $$ (19)

                        $ P_{k-1} = {\rm E}[{{{e}}_{k - 1}}{{e}}_{k - 1}^{\rm T}|Y_c^{k-1}] $, 其迭代關系由下面的(32)和(34) 給出.

                        考慮到無法根據$ \theta {A_d}{{{e}}_{k - 1}} = {{{B}}_e}{{\tilde G}_{k - 1}} $計算出$ {\theta} $, 為了簡化計算, 取$ {\theta} $

                        $$ \begin{array}{l} {\theta ^ * } = \sqrt {\dfrac{{{\rm {trace}}({Q_{1,0}})}}{{{\rm {trace}}({P_{0}})}}} . \end{array} $$

                        對應地, 取

                        $$ \begin{split} P_k^- =& (1 + {\theta^* }){A_d}{P_{k - 1}}{A_d}^{\rm T}+\\ &(1 + \frac{1}{{{\theta^* }}}){Q_{1,k - 1}} + {Q_{2,k - 1}}. \end{split} $$

                        如果敏感器的測量不受飽和約束的限制, 測量更新可以寫為如下形式:

                        $$ \begin{split}{ {\hat{X}}}_k^ * = &{\rm E}[{{X}}_k|Y_c^k] =\\ &{ {\hat{X}}}_k^ - + {{{K}}_k}({y_k} - {{{C}}}{ {\hat{X}}}_k^ - -Du_k). \end{split}$$ (20)

                        此時的估計誤差為

                        $$ \begin{array}{l} {{e}}_k^ * = {{{X}}_k} - {\hat{X}}_k^ * = (\mathbb{I} - {{{K}}_k}{{{C}}}){{e}}_k^ - - {{{K}}_k}(Dw_k+{d_{k}}). \end{array} $$

                        估計誤差的協方差矩陣為

                        $$ \begin{split} P_k^ * =& \; {\rm E}[{{{e}}_k^ *({{e}}_k^ * )^{\rm T}}|Y_c^{k}] = \\ &(\mathbb{I} - {{{K}}_k}{{{C}}}){\rm E}[{{{e}}_k^ -({{e}}_k^ - )^{\rm T}}|Y_c^{k}]{(\mathbb{I} - {{{K}}_k}{{{C}}})^{\rm T}}+\\ &{{{K}}_k}{\rm E}[d_{k}^2|Y_c^{k}]{{K}}_k^{\rm T} + D^2{{{K}}_k}{\rm E}[w_{k}^2|Y_c^{k}]K_k^{\rm T} \le \\ & (\mathbb{I} - {{{K}}_k}{{{C}}})P_k^ -{(\mathbb{I} - {{{K}}_k}{{{C}}})^{\rm T}} +\\ &{{{K}}_k}R_k{{K}}_k^{\rm T} + D^2{{{K}}_k}S_k{{K}}_k^{\rm T}. \end{split} $$

                        為了便于迭代, 取

                        $$ \begin{split} P_k^ * \; = & \; \; (\mathbb{I} - {{{K}}_k}{{{C}}})P_k^ -{(\mathbb{I} - {{{K}}_k}{{{C}}})^{\rm T}}+\\ &\;\; {{{K}}_k}R_k{{K}}_k^{\rm T} + D^2{{{K}}_k}S_k{{K}}_k^{\rm T}. \end{split} $$ (21)

                        為了取得更好的估計效果, 選擇$ {{K}}_k $ 使$ {\rm{trace}}({P^*_{k}}) $取到最小. 通過求解$ \dfrac{{{\rm d}}}{{{\rm d}{{{K}}_{k}}}}{\rm{trace}}({P^*_{k}}) = 0 $, 得到:

                        $$ {{{K}}_k} = P_k^ - {{{{C}}}^{\rm T}}{({{{C}}}P_k^ - {{{{C}}}^{\rm T}} + R_k+ D^2S_k)^{ - 1}}. $$ (22)

                        當敏感器測量受飽和約束時, 若測量值位于飽和約束區間內, 測量更新仍然可以按照(20)-(22)進行, 但對于量程外的集值信息則不再適用. 對$ a_k $$ b_k $按照如下方式取值:

                        $$ \left\{ \begin{array}{ll} {\text{若}}{y_{c,k}} = {\underline{y}}, \; {\text{則}}\; {a_k} = - \infty,{b_k} = {\underline{y}};\\ {\text{若}}{y_{c,k}} = {{\bar y}},\; {\text{則}}\; {a_k} = {{\bar y}},{b_k} = \infty. \end{array} \right. $$ (23)

                        假設$ {{{X}}_k} $關于$ Y_c^{k-1} $的條件密度為正態的[27], 即有

                        $$ \varphi({{{X}}_k}|Y_c^{k - 1}) = {\cal{N}}({{{X}}_k}; {\hat{X}}_k^ - ,P_{k - 1}^ - ). $$ (24)

                        于是, 可得

                        $$ \begin{split} {{ {\hat{X}}}_k} = & {\rm E}[{{{X}}_k}|Y_c^k] = {\rm E}[{{{X}}_k}|Y_c^{k - 1},{y_{c,k}}]= \\ & {\rm E}[{\rm E}[{{{X}}_k}|Y_c^{k - 1},{y_k}]|Y_c^{k - 1},{y_{c,k}}]=\\ & {\hat{X}}_k^ - + {{{K}}_k}({\rm E}[{y_k}|Y_c^{k - 1},{y_{c,k}}] - {{{C}}} {\hat{X}}_k^ - -Du_k)=\\ & {\hat{X}}_k^ - + {{{K}}_k}({\rm E}[{y_k}|Y_c^{k}] - {{{C}}} {\hat{X}}_k^ - -Du_k). \end{split} $$

                        下面計算

                        $$ {\rm E}[{y_k}|Y_c^{k}] = \int_{{a_k}}^{{b_k}} {{y_k}\varphi ({y_k}|Y_c^{k}){\rm d}} {y_k}. $$ (25)

                        根據貝葉斯定理可知

                        $$\begin{split} \varphi ({y_k}|Y_c^{k}) = & \varphi ({y_k}|Y_c^{k - 1},{y_{c,k}}) = \\ &\frac{{\varphi ({y_{c,k}}|Y_c^{k - 1},{y_k})\varphi ({y_k}|Y_c^{k - 1})}}{{\varphi ({y_{c,k}}|Y_c^{k - 1})}}. \end{split} $$ (26)

                        根據(24)有: $ \hat y_k^ - = {\rm E}[{y_k}|Y_c^{k - 1}] = {{{C}}} {\hat{X}}_k^ - +Du_k. $ 進而可知

                        $$ \begin{split} {S_{y,k}} = & {\rm E}[{({y_k} - \hat y_k^ - )^2}|Y_c^{k - 1}] =\\ & {{C}}P_k^ - {{{C}}^{\rm T}} + {\rm E}[d_{k}^2|Y_c^{k-1}]+ D^2{\rm E}[w_{k}^2|Y_c^{k-1}]. \end{split} $$

                        實際中, 因為$ {\rm E}[d_{k}^2|Y_c^{k-1}] $$ D^2{\rm E}[w_{k}^2|Y_c^{k-1}] $無法計算, 在算法運行中只能用其上界代替, 即取

                        $$ {S_{y,k}} = {{C}}P_k^ - {{{C}}^{\rm T}} +R_k+ D^2S_k. $$ (27)

                        由此得出: $ \varphi ({y_k}|Y_c^{k - 1}) = {\cal{N}}({y_k};\hat y_k^ - ,{S_{y,k}}). $ 進一步可以得到:

                        $$ \Pr({y_{c,k}}|Y_c^{k - 1}) = \int_{{a_k}}^{{b_k}} {\varphi ({y_k}|Y_c^{k - 1}){\rm d}} {y_k}. $$ (28)

                        再注意到

                        $$ \begin{array}{l} \Pr({y_{c,k}}|Y_c^{k - 1},{y_k}) = \left\{ \begin{array}{ll} 1,\;\;{y_k} \in (-\infty, \underline{y}]\cup [\bar{y},\infty);\\ 0,\;\;{\text{其它}}. \end{array} \right.\; \; \end{array} $$

                        將上式和(24)(28)代入(26)得到:

                        $$ \begin{split} & {\varphi ({y_k}|Y_c^{k - 1},{y_{c,k}})}=\\ &\quad \left\{ \begin{array}{ll} {\dfrac{{\varphi ({y_k}|Y_c^{k - 1})}}{{\Pr({y_{c,k}}|Y_c^{k - 1})}}},\; \; {y_k} \in (-\infty, \underline{y}]\cup [\bar{y},\infty);\\ {\;\;\;\;\;\;0},\; \; {\text{其它}}. \end{array} \right. \end{split} $$

                        所以, 根據(25)我們有

                        $$ \begin{split} {{\rm E}[{y_k}|Y_c^{k}]}= & \dfrac{{\int_{{a_k}}^{{b_k}} {{y_k}\varphi ({y_k}|Y_c^{k - 1}){\rm d}} {y_k}}}{{\int_{{a_k}}^{{b_k}} {\varphi ({y_k}|Y_c^{k - 1}){\rm d}} {y_k}}} ={{C}} {\hat{X}}_k^- + Du_k +\\ & \dfrac{{Z(\dfrac{{{a_k} - \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }}) - Z(\dfrac{{{b_k} - \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }})}}{{\Phi (\dfrac{{{b_k} - \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }}) - \Phi (\dfrac{{{a_k} - \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }})}}\sqrt {{S_{y,k}}},\\[-30pt] \end{split} $$ (29)

                        其中$ a_k $$ b_k $由(23)給出; $ S_{y,k} $由(27) 給出; $ Z(t) = $ $ \dfrac{1}{{\sqrt {2{\text{π}}} }}\exp\{{ - \dfrac{1}{2}{t^2}}\} $, $ \Phi(t) = $ $ \dfrac{1}{{\sqrt {2{\text{π}} } }}\int_{ - \infty }^t \exp\{{ - \dfrac{1}{2}{s^2}}\}{\rm d}s $.

                        根據$ {{{e}}_k} = \; {{{X}}_k} - {{ {\hat{X}}}_k} = \; {{{X}}_k} - {\hat{X}}_k^ * + {\hat{X}}_k^ * - {{ {\hat{X}}}_k} $,

                        $$ \begin{array}{l} {{{e}}_k}\; \; = \; {{{X}}_k} - {\hat{X}}_k^ * + {{{K}}_k}({y_k} - {\rm E}[{y_k}|Y_c^{k}]), \end{array} $$

                        得到 $ {P_k} = {\rm E}[{{{e}}_k^ * ({{e}}_k^ * )^{\rm T}}|Y_c^{k}]+ {{{K}}_k}{\mathop{\rm {cov}}}[{y_k}|Y_c^{k}]{{K}}_k^{\rm T}. $ 由(21)得

                        $$ {P_k} = P_k^ * + {{{K}}_k}{\mathop{\rm {cov}}}[{y_k}|Y_c^{k}]{{K}}_k^{\rm T}, $$

                        其中$ {\rm{cov}}[{y_k}|Y_c^{k}] $由(30)給出(見下頁頂端).

                        $$ \begin{split} {\rm{cov}}&[{y_k} |Y_c^{k}] = {\rm{cov}}[{y_k}|Y_c^{k - 1},{y_{c,k}}] =\\ & {\rm E}({y_k} - {\rm E}[{y_k}|Y_c^{k - 1},{y_{c,k}}])^2=\\ & {\rm E}[y_k^2|Y_c^{k - 1},{y_{c,k}}] - {({\rm E}[{y_k}|Y_c^{k - 1},{y_{c,k}}])^2} =\\ &\dfrac{{\int_{{a_k}}^{{b_k}} {y_k^2\varphi ({y_k}|Y_c^{k - 1})d} {y_k}}}{{\int_{{a_k}}^{{b_k}} {\varphi ({y_k}|Y_c^{k - 1})d} {y_k}}} - {({\rm E}[{y_k}|Y_c^{k - 1},{y_{c,k}}])^2} = \\ & \left[ {1 \!+\! \dfrac{{\dfrac{{{a_k} \!-\! \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }}Z(\dfrac{{{a_k} \!-\! \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }}) \!-\! \dfrac{{{b_k} \!-\! \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }}Z(\dfrac{{{b_k} \!-\! \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }})}}{{\Phi (\dfrac{{{b_k} \!-\! \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }}) \!-\! \Phi (\dfrac{{{a_k} \!-\! \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }})}}\!-} \right.\\ & \left.{ {{\left( {\dfrac{{Z(\dfrac{{{a_k} - \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }}) - Z(\dfrac{{{b_k} - \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }})}}{{\Phi (\dfrac{{{b_k} - \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }}) - \Phi (\dfrac{{{a_k} - \hat y_k^ - }}{{\sqrt {{S_{y,k}}} }})}}} \right)}^2}} \right]{S_{y,k}}.\\[-30pt] \end{split} $$ (30)

                        總結以上, 在飽和約束測量(14)下系統(13)的擴張狀態濾波算法如下:

                        算法2: 測量飽和受限下的擴張狀態濾波算法

                        初始化:

                        給定初值$ {{ {\hat{X}}}_0},{P_0} $.

                        時間更新:

                        $$ \begin{split} {\hat{X}}_k^- =& {A_d}{{ {\hat{X}}}_{k - 1}} + {{{B}}_d}{u_{k - 1}} + {{{B}}_e}{{\hat G}_{k - 1}},\\ P_k^- =& (1 + {\theta^* }){A_d}{P_{k - 1}}{A_d}^{\rm T}+\\ &(1 + \frac{1}{{{\theta^* }}}){Q_{1,k - 1}} + {Q_{2,k - 1}}, \end{split} $$

                        其中$ {{\hat G}_{k - 1}} $、$ {Q_{1,k - 1}} $$ {Q_{2,k - 1}} $分別由(9)、(18)和(19)給出.

                        測量更新:

                        (I)如果$ y_{c,k}\in(\underline{y},\bar{y}) $(即$ y_k $的取值在量程范圍內), 那么有

                        $$ {\hat{X}}_k = {\hat{X}}_k^ - + {{{K}}_k}({y_k} - {{C}} {\hat{X}}_k^ - -Du_k), \quad$$ (31)
                        $$ \begin{split} P_k =& P_k^* = (\mathbb{I} - {{{K}}_k}{{{C}}})P_k^-{(\mathbb{I} - {{{K}}_k}{{{C}}})^{\rm T}}+\\ &{{{K}}_k}R_k{{K}}_k^{\rm T}+D^2{{{K}}_k}S_k{{K}}_k^{\rm T}.\end{split}$$ (32)

                        (II)如果$ y_{c,k} = \underline{y} $$ y_{c,k} = \bar{y} $(即$ y_k $的取值超出了測量量程), 那么有

                        $$ {{ {\hat{X}}}_k} = {\hat{X}}_k^ -+{{{K}}_k}({\rm E}[{y_k}|Y_c^{k}]- {{{C}}} {\hat{X}}_k^ - -Du_k),\; \; \; \; \; \; \; $$ (33)
                        $$ P_k = P_k^*+{{{K}}_k}{\mathop{\rm {cov}}} [{y_k}|Y_c^{k}]{{K}}_k^{\rm T},\qquad\qquad\qquad\qquad $$ (34)

                        其中$ {{K}}_k $, $ {\rm E}[{y_k}|Y_c^{k}] $, $ {\mathop{\rm {cov}}} [{y_k}|Y_c^{k}] $ 分別由(22), (29), (30)給出.

                        注 2.3 算法2的測量更新階段分成了兩種情況, 這使得難以像經典Kalman濾波那樣寫出預測誤差協方差矩陣滿足的Riccati方程[31]; 另一方面, 即便是采用分類函數的方法從形式上寫出一個Riccati方程的樣子, 因為約束區間外的集值信息關于測量是高度非線性的, 這樣的方程不可避免地包含了形如(29)(30)那樣的強非線性項, 這都使得穩定性分析變得十分困難.

                      • $ { {\hat{X}}_{1,k}}, { {\hat{X}}_{2,k}} $分別為$ k $時刻由位移敏感器和加速度計得到的擴張狀態估計結果, 即

                        $$ { {\hat{X}}_{1,k}} = \Gamma{ {\hat{X}}^{\{1\}}_{1,k}}, $$ (35)

                        其中$ \Gamma = \left[ {\begin{array}{*{20}{c}} {{0_{1 \times 9}}}&{ {\Upsilon}}&{{0_{1 \times 15}}}\\ {{0_{1 \times 12}}}&{ {\Upsilon}}&{{0_{1 \times 12}}}\\ {{0_{1 \times 15}}}&{ {\Upsilon}}&{{0_{1 \times 9}}} \end{array}} \right] $, $ { {\Upsilon}} $的取值由系統(10)所考慮的方向決定, 具體如表1所示; $ { {\hat{X}}^{\{1\}}_{1,k}} $由算法1給出, 其協方差矩陣記為$ P_{1,k}^{\{1\}} $; $ { {\hat{X}}_{2,k}} $由算法2給出. $ {P_{1,k}},{P_{2,k}} $分別為相應的估計誤差協方差矩陣. 于是, 由(35)可知,

                        表 1  ${ {\Upsilon}}$矩陣

                        Table 1.  The matrix of ${ {\Upsilon}}$

                        X方向 Y方向 Z方向
                        $\left[ {\begin{array}{*{20}{c}} 1&0&0 \end{array}} \right]$ $\left[ {\begin{array}{*{20}{c}} 0&1&0 \end{array}} \right]$ $\left[ {\begin{array}{*{20}{c}} 0&0&1 \end{array}} \right]$
                        $$ P_{1,k} = \Gamma{P_{1,k}^{\{1\}}}\Gamma^{\rm T}. $$ (36)

                        注意到協方差矩陣的對角線元素衡量的是相關變量的估計效果, 非對角線上的元素刻畫的是變量間的相關性. 我們關心的是如何提高算法的估計精度, 即使得協方差矩陣的跡變小. 加速度計的測量精度很高, 但僅能檢測到無拖曳子系統的殘余加速度信息, 而速度信息是加速度在時間上的一重積分、位移信息是加速度在時間上的二重積分. 隨著遞推次數的增加, 估計誤差的累積必然會影響對速度和位移的估計. 為此, 我們用算法1對速度和位移進行估計, 用算法2對干擾進行估計, 并把融合后的結果返給算法1和算法2進行遞推. 具體地, 取:

                        $$ \label{ox} {{ {\hat{X}}}_{o,k}} = \Lambda_1{ {\hat{X}}_{1,k}}+\Lambda_2{ {\hat{X}}_{2,k}}, $$

                        其中$ { {\hat{X}}_{o,k}} $表示對擴張狀態的融合估計結果, 加權矩陣

                        $$ \Lambda_1 = \left[ \begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \\ \end{array} \right],\; \; \Lambda_2 = \left[ \begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{array} \right]. $$ (37)

                        相應地, 估計誤差協方差矩陣取為

                        $$ {P_{o,k}} = \Lambda_1P_{1,k}+\Lambda_2P_{2,k}. $$

                        然后把$ {{ {\hat{X}}}_{o,k}} $$ {P_{o,k}} $返給算法1和算法2進行下一步的遞推計算.

                        綜上, 融合算法如下:

                        算法3: 融合算法

                        初始化: 給定算法1的初值$ {\hat{X}}_{1,0}^{\{1\}} $$ {P_{1,0}^{\{1\}}} $, 以及算法2的初值$ {{ {\hat{X}}}_{2,0}} $$ {P_{2,0}} $.

                        第一步: 運行算法1得到$ { {\hat{X}}^{\{1\}}_{1,k}} $$ {P_{1,k}^{\{1\}}} $, 然后根據(35)和(36)計算$ { {\hat{X}}_{1,k}} $$ P_{1,k} $; 運行算法2得到$ { {\hat{X}}_{2,k}} $$ P_{2,k} $.

                        第二步: 融合$ { {\hat{X}}_{1,k}} $$ { {\hat{X}}_{2,k}} $得到$ { {\hat{X}}_{o,k}} $$ {P_{o,k}} $:

                        $$ {{ {\hat{X}}}_{o,k}} = \Lambda_1{ {\hat{X}}_{1,k}}+\Lambda_2{ {\hat{X}}_{2,k}}; $$ (38)
                        $$ {P_{o,k}} = \Lambda_1P_{1,k}+\Lambda_2P_{2,k},\quad $$ (39)

                        其中加權系數$ \Lambda_1 $$ \Lambda_2 $由(37)給出.

                        第三步: 把融合后的結果返給算法1和算法2, 具體地有,

                        $$ {\hat{X}}_{1,k}^{\{ 1\} } = (\mathbb{I}- {\Gamma ^{\rm T}}\Gamma ) {\hat{X}}_{1,k}^{\{ 1\} } + {\Gamma ^ + }{{ {\hat{X}}}_{o,k}}; $$ (40)
                        $$\begin{split} P_{1,k}^{\{ 1\} } =& (\mathbb{I} - {\Gamma ^{\rm T}}\Gamma )P_{1,k}^{\{ 1\} }{(\mathbb{I} - {\Gamma ^{\rm T}}\Gamma )^{\rm T}}+\;\\ &{\Gamma ^ + }{P_{o,k}}{({\Gamma ^ + })^{\rm T}}; \end{split} $$ (41)
                        $$ {{ {\hat{X}}}_{2,k}} = {{ {\hat{X}}}_{o,k}}; \qquad\qquad\qquad \qquad\quad\,$$ (42)
                        $$ {P_{2,k}} = {P_{o,k}}, \qquad\qquad\qquad\qquad \quad\;\;\;$$ (43)

                        其中, 上標$ ^+ $表示一個矩陣的Moore-Penrose廣義逆.

                        注 2.4 根據矩陣$ \Gamma $的形式, 由(35)和(36)可以看出$ {\hat{X}}_{1,k} $$ P_{1,k} $$ {\hat{X}}^{\{1\}}_{1,k} $$ P^{\{1\}}_{1,k} $的子式, 式(40)和(41)的操作過程實際上是在$ {\hat{X}}^{\{1\}}_{1,k} $$ P^{\{1\}}_{1,k} $ 中把$ {\hat{X}}_{1,k} $$ P_{1,k} $更新成了$ {\hat{X}}_{o,k} $$ P_{o,k} $.

                        算法3的運行過程可簡單歸結為如下示意圖:

                        圖  4  融合算法示意圖

                        Figure 4.  Diagram of the fusion algorithm

                      • 由(1)-(3), 可以看出系統存在以下三種類型的耦合: 子系統間的耦合作用, 衛星本體姿態控制子系統與衛星-測試質量相對姿態控制子系統間存在耦合關系, 即調節衛星本體姿態角的同時影響著衛星與測試質量間的相對姿態; 子系統內部各個方向上的控制輸入對系統狀態的調節具有耦合作用, 即系統內某個方向的控制輸入會對其它方向上的狀態帶來影響(例衛星本體姿態的輸入系數矩陣$ I_{{\rm{sc}}}^{ - 1} $非對角線上的元素不為零, 這說明對某一方向的控制輸入不僅會改變本方向的狀態還會對其它方向的狀態造成影響); 子系統內部各個狀態的耦合作用, 由于$ {K_{{\rm{trans}}}} $、$ {D_{{\rm{trans}}}} $的存在, 無拖曳控制系統的各個狀態間存在著相互影響(例如衛星運行切線方向上的速度增加, 則會使得衛星做離心運動, 進而影響徑向的無拖曳控制效果), 同理由于$ {K_{{\rm{rot}}}} $、$ {D_{{\rm{rot}}}} $的存在, 衛星-測試質量相對姿態控制子系統的各個狀態間也存在著相互影響. 結合$ f,w,d $的存在, 所以無拖曳衛星的本體姿態–衛星本體與測試質量相對位移–衛星本體與測試質量相對姿態的聯合控制需要克服干擾、噪聲、耦合三大方面的問題.

                        本文使用擴張狀態濾波來克服噪聲問題并實現對擾動的估計; 對于系統存在的耦合問題, 我們借鑒自抗擾的思想, 在補償掉外部擾動之后, 將系統狀態間的耦合關系看成內部擾動并進行補償, 使得被控系統等價為“積分串聯型系統”, 以便于實現解耦控制.

                        對于衛星本體姿態控制子系統, $ {T_{Dsc}} $是系統的外部干擾, 衛星本體姿態的輸入系數矩陣$ I_{{\rm{sc}}}^{ - 1} $非對角線上的元素不為零意味著系統內某個方向的控制輸入會對其它方向的狀態帶來耦合影響, 于是在產生控制作用時, 一方面要將系統的外部干擾抵消掉, 另一方面將系統的控制輸入解耦, 對此控制量取

                        $$ {{{T}}_{Csc}} = {I_{{\rm{sc}}}}{{{T}}_{Csc0}} - {{ {\hat{T}}}_{Dsc}}, $$ (44)

                        其中: $ {{{T}}_{Csc}} $為控制器最終希望輸出的控制量, $ {{{T}}_{Csc0}} $為NLPID計算出的控制量, $ {{ {\hat{T}}}_{Dsc}} $表示對衛星所受擾動力矩$ {{{T}}_{Dsc}} $的估計值.

                        將(44)代入(1)有

                        $$ \begin{split} {{\ddot {{\varphi}} }_{{\rm{sc}}}} =& I_{{\rm{sc}}}^{ - 1}( {{{{T}}_{Csc}} + {{{T}}_{Dsc}} + {{{{w}}_{{T_{Csc}}}}}} ) =\\ &I_{{\rm{sc}}}^{ - 1}( {{I_{{\rm{sc}}}}{{{T}}_{Csc0}} - {{ {\hat{T}}}_{Dsc}} + {{{T}}_{Dsc}} + {{{w}}_{{T_{Csc}}}}} ) \approx\\ &I_{{\rm{sc}}}^{ - 1}( {{I_{{\rm{sc}}}}{{{T}}_{Csc0}}} ) \approx {{{T}}_{Csc0}}, \end{split} $$

                        即實現了衛星本體姿態的解耦控制.

                        對于無拖曳控制子系統, $ {{{F}}_{Dsc}} $是系統的外部干擾, 系統內部狀態間的耦合項$ ( - \frac{{{K_{{\rm{trans}}}}}}{{{m_{{\rm{tm}}}}}}{{{r}}_{{\rm{rel}}}} - \frac{{{D_{{\rm{trans}}}}}}{{{m_{{\rm{tm}}}}}}{{\dot {{r}}}_{{\rm{rel}}}} ) $可以視為系統所受的“內部干擾”, 將無拖曳控制子系統的“內部干擾”記為$ {{h}}_{idf} $. 于是在產生控制作用時, 一方面要將系統的外部干擾抵消掉, 另一方面將系統的“內部干擾”抵消掉, 對此控制量取

                        $$ {{{F}}_{Csc}} = - {{{F}}_{Csc0}} + {m_{{\rm{sc}}}}{{ {\hat{h}}}_{idf}} - {{ {\hat{F}}}_{Dsc}}, $$ (45)

                        其中: $ {{{F}}_{Csc}} $為控制器最終希望輸出的控制量, $ {{{F}}_{Csc0}} $為NLPID計算出的控制量, $ {{ {\hat{h}}}_{idf}} $表示對$ {{{{h}}}_{idf}} $的估計值($ {{ {\hat{h}}}_{idf}} = - \dfrac{{{K_{{\rm{trans}}}}}}{{{m_{{\rm{tm}}}}}}{ {\hat{r}}_{{\rm{rel}}}} - \dfrac{{{D_{{\rm{trans}}}}}}{{{m_{{\rm{tm}}}}}}{{\hat {\dot {{r}}}}_{{\rm{rel}}}} $), $ {{ {\hat{F}}}_{Dsc}} $表示對$ {{F}}_{Dsc} $的估計值.

                        將(45)代入(2)有

                        $$ \begin{split} {{\ddot {{r}}}_{{\rm{rel}}}} = & {{{{h}}}_{idf}} - \dfrac{\mathbb{I}_3}{{{m_{{\rm{sc}}}}}}( {{{{F}}_{Csc}} + {{{{w}}_{{F_{Csc}}}}} + {{{F}}_{Dsc}}}) ={{{h}}_{idf}} - \\ &\dfrac{{\mathbb{I}_3}}{{{m_{{\rm{sc}}}}}}( {{m_{{\rm{sc}}}}{{ {\hat{h}}}_{idf}}\!-\! {{{F}}_{Csc0}} \!-\! {{ {\hat{F}}}_{Dsc}} \!+\! {{{w}}_{{{F}_{Csc}}}} \!+\! {{{F}}_{Dsc}}} ) \approx\\ &{{{h}}_{idf}} - \dfrac{{\mathbb{I}_3}}{{{m_{{\rm{sc}}}}}}( {{m_{{\rm{sc}}}}{{ {\hat{h}}}_{idf}} - {{{F}}_{Csc0}}} ) \approx\\ &\dfrac{{{\mathbb{I}_3}}}{{{m_{{\rm{sc}}}}}} {{{{F}}_{Csc0}}}, \end{split} $$

                        即實現了衛星無拖曳系統的解耦控制.

                        對于衛星與測試質量間的相對姿態控制子系統, 系統內部狀態間的耦合項$ ( I_{{\rm{tm}}}^{ - 1}{K_{{\rm{rot}}}}{{{\varphi}} _{{\rm{rel}}}}\! +\! I_{{\rm{tm}}}^{ - 1}{D_{{\rm{rot}}}}{{\dot {{\varphi}} }_{{\rm{rel}}}} ) $可以視為系統所受的“內部干擾”, 將相對姿態控制子系統的“內部干擾”記為$ {{h}}_{itm} $, $ {{{T}}_{Dtm}} $是系統的外部干擾, $ I_{{\rm{sc}}}^{ - 1}\left( {{{{T}}_{Csc}} + {{{T}}_{Dsc}} + {{{w}}_{{T_{Csc}}}}} \right) = {{\ddot {{\varphi}} }_{{\rm{sc}}}} $ 是衛星姿態控制子系統在調整衛星姿態時對衛星本體與測試質量間的相對姿態變化的影響因素. 在產生控制作用時, 不僅將系統的外部干擾抵消掉, 還要將系統的“內部干擾”抵消掉, 最終控制量取

                        $$ {{{T}}_{Ctm}} = {I_{{\rm{tm}}}} {{{{T}}_{Ctm0}}} - {{ {\hat{T}}}_{Dtm}} - {I_{{\rm{tm}}}}{{ {\hat{h}}}_{itm}}, $$ (46)

                        其中: $ {{{T}}_{Ctm}} $為控制器最終希望輸出的控制量, $ {{{T}}_{Ctm0}} $為NLPID根據被控量的變化情況產生的控制量, $ {{ {\hat{h}}}_{itm}} $表示對內擾力矩$ {{ {{h}}}_{itm}} $的估計值($ {{ {\hat{h}}}_{itm}} = I_{{\rm{tm}}}^{ - 1}{K_{{\rm{rot}}}}{ {\hat{\varphi}} _{{\rm{rel}}}} + I_{{\rm{tm}}}^{ - 1}{D_{{\rm{rot}}}}{{\hat {\dot {{\varphi}} }}_{{\rm{rel}}}} $), $ {{ {\hat{T}}}_{Dtm}} $表示對外擾力矩$ {{ {{T}}}_{Dtm}} $的估計值.

                        將(46)代入(3)有

                        $$ \begin{split} {{\ddot {{\varphi}} }_{{\rm{rel}}}} = & {{{h}}_{itm}} + I_{{\rm{tm}}}^{ - 1}({{{T}}_{Ctm}} + {{{T}}_{Dtm}} + {{{w}}_{{T_{Ctm}}}}) - {{\ddot {{\varphi}} }_{{\rm{sc}}}} =\\ &{{{h}}_{itm}} + I_{{\rm{tm}}}^{ - 1}({I_{{\rm{tm}}}}{{{T}}_{Ctm0}} - {{ {\hat{T}}}_{Dtm}} - {I_{{\rm{tm}}}}{{ {\hat{h}}}_{itm}} +\\ &{{{T}}_{Dtm}} + {{{w}}_{{T_{Ctm}}}}) - {{\ddot {{\varphi}} }_{{\rm{sc}}}} \approx\\ &{{{h}}_{itm}} + I_{{\rm{tm}}}^{ - 1}({I_{{\rm{tm}}}}{{{T}}_{Ctm0}} - {I_{{\rm{tm}}}}{{ {\hat{h}}}_{itm}}) - {{\ddot {{\varphi}} }_{{\rm{sc}}}} \approx\\ &{{{T}}_{Ctm0}} - {{\ddot {{\varphi}} }_{{\rm{sc}}}}, \end{split} $$

                        對于$ {\ddot \varphi _{{\rm{sc}}}} $可以看做是系統受到的階躍擾動, 通過適當增加非線性PID中微分環節的的作用即可以降低該影響, 這樣就實現了對相對姿態的調節.

                        注 2.5 考慮到執行器的能力, 實際中的控制律(44)、(45)和(46)是受限的, 也就是存在常數$ \rho_1>0 $、$ \rho_2>0 $$ \rho_3>0 $使得

                        $$ {{T}}_{Csc}\in {\cal{B}}(\rho_1),\; {{F}}_{Csc}\in {\cal{B}}(\rho_2),\; {{T}}_{Ctm}\in {\cal{B}}(\rho_3), $$

                        其中$ {\cal{B}}(\rho) $表示三維空間中邊長為$ \rho $的立方體, 即$ {\cal{B}}(\rho) = \{(x_1,x_2,x_3)\in{\bf R}^3: -\rho\leq x_i\leq \rho, i = 1,2,3\} $.

                      • 本節對前面的擴張狀態估計算法和控制律進行數值仿真, 系統模型(1)-(3)中的參數取自文[29]和[30], 如表2所示.

                        表 2  系統仿真參數

                        Table 2.  System parameters in the simulation

                        變量數值
                        ${m_{{\rm{tm}}}}$1 kg
                        ${m_{{\rm{sc}}}}$1 050 kg
                        ${I_{{\rm{tm}}}}$$0.2667 \times {10^{ - 3}}{\mathbb{I}_3}({\rm{kg}} \cdot {{\rm{m}}^{\rm{2}}})$
                        ${I_{{\rm{sc}}}}$$\left[ {\begin{array}{*{20}{c}} {200}&1&2\\ 1&{2\;700}&1\\ 2&1&{2\;650} \end{array}} \right]({\rm{kg}} \cdot {{\rm{m}}^{\rm{2}}})$
                        ${K_{{\rm{trans}}}}$$\left[ {\begin{array}{*{20}{c}} 1&{0.039}&{0.039}\\ {0.039}&1&{0.039}\\ {0.039}&{0.039}&1 \end{array}} \right] \times {10^{ - 6}}{\rm{(N/m)}}$
                        ${D_{{\rm{trans}}}}$$1.4 \times {10^{ - 11}}{\mathbb{I}_3}({\rm{N}} \cdot {\rm{m}} \cdot {\rm{s/rad}})$
                        ${K_{{\rm{rot}}}}$$\left[ {\begin{array}{*{20}{c}} 1&{10}&{10}\\ {10}&1&{10}\\ {10}&{10}&1 \end{array}} \right] \times {10^{ - 9}}({\rm{N}} \cdot {\rm{m/rad}})$
                        ${D_{{\rm{rot}}}}$$1.4 \times {10^{ - 11}}{I_3}({\rm{N}} \cdot {\rm{m}} \cdot {\rm{s/rad}})$
                        ${{{T}}_{Dsc}}$$ \left[ {\begin{array}{*{20}{c} } { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{t} })}\\ { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{ + } }\dfrac{ {2{\text{π}} } }{3})}\\ { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{ + } }\dfrac{ {4{\text{π}} } }{3})} \end{array} } \right]({\rm{mN} } \cdot {\rm{m} })$
                        ${{{F}}_{Dsc}}$$ \left[ {\begin{array}{*{20}{c} } { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{t} })}\\ { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{ + } }\dfrac{ {2{\text{π}} } }{3})}\\ { - {\rm{12} }.{\rm{8 + 7} }.{\rm{7sin} }({\omega _d}{\rm{ + } }\dfrac{ {4{\text{π}} } }{3})} \end{array} } \right]({\rm{mN} })t]({\rm{mN} })$
                        ${{{T}}_{Dtm}}$0
                        $\omega_d$$1.2 \times {10^{ - 3}}\;{\rm{Hz}}$

                        敏感器采樣以及執行器執行周期$ h = {\rm{0.1s}} $, 輸入噪聲$ {{{w}}_{{T_{Csc}}}} $$ {{{w}}_{{F_{Csc}}}} $的功率譜密度為$ 1 \times {10^{ - 8}}{\rm{N}} \cdot $$ {{\rm{s}}^{ - 2}}/\sqrt {{\rm{Hz}}} $, $ {{{w}}_{{T_{Ctm}}}} $的功率譜密度為$ 1 \times {10^{ - 15}}{\rm{N}} \cdot {{\rm{s}}^{ - 2}}/ $$\sqrt {{\rm{Hz}}} $, 位移和姿態敏感器測量噪聲的功率譜密度為$ 1 \times {10^{ - 8}} {\rm{m}}/\sqrt {{\rm{Hz}}} $, 加速度計測量噪聲的功率譜密度為$ 1 \times {10^{ - 12}} {\rm{m}}/\sqrt {{\rm{Hz}}} $. 無拖曳子系統中, 加速度計測量下界$ \underline y \!=\! -6 \times {10^{ - 6}}{\rm{m}} \cdot {{\rm{s}}^{ - 2}} $, 上界$ {\bar y} \!=\! 6 \times {10^{ - 6}}{\rm{m}} \cdot {{\rm{s}}^{ - 2}} $. 擾動的標稱模型$ \bar{{{f}}} = 0 $. $ \rho_1 = \rho_2 = \rho_3 = 0.03{\rm{N}} $. 假設四中的$ {{{q}}_{k,i}} = {(\max(\dfrac{{{\rm d}{{f}}_i}}{{{\rm d}t}}){{h)}}^2} $. 算法1-3中估計初值取為0, 估計誤差的協方差初值取為對角線元素全為0.01的對角矩陣.

                        控制器相關參數取值如下: $ {{{T}}_{Csc0,{\rm{sc\_a}}}} = $$ 0.001{\rm{fal}}\;({{{e}}_{{\rm{p}},{\rm{sc\_a}}}})+0.0001{\rm{fal}}\;({{{e}}_{{\rm{i}},{\rm{sc\_a}}}})+0.01{\rm{fal}}\;({{{e}}_{{\rm{d}},{\rm{sc\_a}}}})$, $ {{{e}}_{{\rm{p}},{\rm{sc\_a}}}} = 0 - {{ {\hat{\varphi}} }_{{\rm{sc\_a}},{\rm{k}}}} $, $ {{{e}}_{i,{\rm{sc\_a}}}} = \displaystyle\sum\limits_{j = 0}^k {0 - {{ {\hat{\varphi}} }_{{\rm{sc\_a}},j}}} $, ${{{e}}_{{\rm{d}},{\rm{sc\_a}}}} = $$ 0 - {{\hat {\dot {{{\varphi}}}} }_{{\rm{sc\_a}},{\rm{k}}}} $, $ {\rm{sc\_a}} \in \{ {\rm{pitch,yaw,roll\} }} $; $ {{{F}}_{Csc0,{\rm{rel\_p}}}} \!=\!0.5{\rm{fal}}$$ ({{{e}}_{{\rm{p}},{\rm{rel\_p}}}})+0.01{\rm{fal}}({{{e}}_{{\rm{i}},{\rm{rel\_p}}}})+1.9{\rm{fal}}({{{e}}_{{\rm{d}},{\rm{rel\_p}}}}), $ $ {{{e}}_{{\rm{p}},{\rm{rel\_p}}}} = 0 -$$ {{ {\hat{r}}}_{{\rm{rel\_p}},{\rm{k}}}} $, $ {{{e}}_{{\rm{i}},{\rm{rel\_p}}}} = \displaystyle\sum\limits_{j = 0}^k {0 - {{ {\hat{r}}}_{{\rm{rel\_p}},j}}} $, $ {{{e}}_{{\rm{d}},{\rm{rel\_p}}}} = 0 - {{\hat {\dot {{{r}}}}}_{{\rm{rel\_p}},{\rm{k}}}}$, $ {\rm{rel\_p}} \in \{ {\rm{x,y,z\} }} $; ${{{T}}_{Ctm0,{\rm{rel\_a}}}} = 0.{\rm{0001fal}}({{{e}}_{{\rm{p}},{\rm{rel\_a}}}}){{ + }} 0.0021$${\rm{fal}} ({{{e}}_{{\rm{d}},{\rm{rel\_a}}}}), $ $ {{{e}}_{{\rm{p}},{\rm{rel\_a}}}} = 0 - {{ {\hat{\varphi}} }_{{\rm{rel\_a}},{\rm{k}}}} $, $ {{{e}}_{{\rm{d}},{\rm{rel\_a}}}} = 0 - {{\hat {\dot {{{\varphi}}} }}_{{\rm{rel\_a}},{\rm{k}}}} ,$ $ {\rm{rel\_a}} \in \{ {\rm{relpitch,relyaw,relroll\} }} $. 為了書寫方便, 這里將$ {\rm{fal}}(e,0.5,0.01) $簡寫為了$ {\rm{fal}}(e) $.

                        考慮無拖曳子系統在$ X $方向上的運動, 圖5的上圖是加速度計的測量結果, 下圖是對擾動的估計結果, 其中'L1'表示算法2的運行結果; 假設測量不受飽和約束(稱為理想情況), 'L2'給出了算法2在這種情形下的運行結果; 如果只利用約束區間內的測量信息(即當測量超出約束區間時, 估計不更新), 'L3'給出了此時算法2的運行結果, 此時的估計出現了明顯偏差. 由L1和L2的對比, 可以看出測量的飽和約束的確影響到了估計效果; 由L1和L3的對比, 可以看出約束區間外的集值信息盡管有限但仍包含了系統的重要信息, 算法2對集值信息的利用效果明顯.

                        圖  5  算法2對擾動估計效果

                        Figure 5.  Disturbance estimation performance of Algorithm 2

                        算法1和算法2對位移和速度的估計效果如圖6所示, 可以看出: 算法1的估計效果相對穩定; 算法2在遞推次數變大時對估計誤差的累積效果比較明顯, 并影響對擾動的估計精度(見圖7). 算法1是基于整個無拖曳衛星系統中的位移及姿態敏感器測量信息展開的擴張狀態濾波, 這類敏感器測量的是系統的低階狀態(即位移信息相對于速度、加速度等信息是系統的低階狀態). 系統對高階狀態的估計是通過低階狀態的微分運算得到的, 所以利用低階狀態敏感器的測量信息進行擴張狀態濾波時不存在誤差的累積問題, 算法1的估計效果也就相對穩定. 算法2基于的高階狀態敏感器測量精度高, 所以其在初始一段時間內(此時累積誤差相對較小)對擾動的估計效果要優于算法1, 后來由于誤差的累積致使對擾動的估計效果變差. 根據殘余加速度公式, 可以看出由殘余加速度結合相對位移、相對速度及控制輸入等信息才能推出擾動信息, 而誤差的累積使得算法對$ {r_{{\rm{rel}}}},{\dot r_{{\rm{rel}}}} $的估計出現很大的偏差, 所以導致對擾動的估計效果也逐漸變差.

                        圖  6  位移/速度的估計效果: 算法1 vs. 算法2

                        Figure 6.  Estimation of displacement/speed: Algorithm 1 vs. Algorithm 2

                        圖  7  擾動估計效果: 算法1 vs.算法2

                        Figure 7.  Estimation of disturbance: Algorithm 1 vs. Algorithm 2

                        圖7也可看出, 在遞推次數不大的時候, 算法2對擾動的估計效果要優于算法1的效果. 整體來看, 算法1對位移和速度的估計較好, 算法2對擾動的估計較好但需要誤差校正以克服遞推次數變大帶來的累積誤差, 這是對算法1和算法2進行融合的基本出發點. 圖8展示了融合算法3對擾動的估計效果.

                        圖  8  擾動估計效果: 融合算法3 vs. 算法1

                        Figure 8.  Estimation of disturbance: Algorithm 3 (fusion algorithm) vs. Algorithm 1

                        $ X $方向上的無拖曳控制, 圖9對比了PID控制律與ADRC的效果. 在做對比仿真時采用的策略是: 首先多次整定PID控制器的參數, 然后選取多次整定得到的最優參數(比例系數$ {K_{\rm{P}}} = 5.55 $, 積分系數$ {K_{\rm{I}}} = 0 $, 微分系數$ {K_{\rm{D}}} = 9 $) 和ADRC控制器進行對比, 其中ADRC也采用了同樣的PID(即ADRC控制律比前者僅多了一項擾動補償部分). 從圖中可以看出ADRC算法較PID控制算法能更好的抑制周期性擾動.

                        圖  9  X方向上控制效果對比

                        Figure 9.  Comparison of control performance on X direction

                        圖10是采用ADRC后系統穩定運行時衛星本體姿態、衛星與測試質量間相對位移、衛星與測試質量間相對姿態的控制效果.

                        圖  10  系統穩定運行時控制效果

                        Figure 10.  Control performance when the system is running steadily

                        加入階躍指令(在$ k = 1\times{10^{5}} $時, 給定${{{\varphi}} _{{\rm{sc,pitch}}}} = $$ 0.01{\rm{/rad}},{{{\varphi}} _{{\rm{sc,yaw}}}} = 0.02{\rm{/rad}},{{{\varphi}} _{{\rm{sc,roll}}}} = 0.03{\rm{/rad}} $控制指令; 并在$ k = 2\times{10^{5}} $時回到原姿態)后, 圖11顯示衛星本體姿態控制回路能夠較好地達到期望姿態, 同時可以看到衛星本體姿態的調整會對衛星與測試質量間的相對姿態產生影響, 不過相對姿態控制回路能夠迅速的進行調整.

                        圖  11  姿態調整效果

                        Figure 11.  Results of attitude adjustment

                        圖12是在ADRC下測試質量上平動殘余加速度的功率譜密度曲線, 可以看出作用在檢測質量上的非保守力加速度的功率譜密度(PSD)在測量帶寬($ 1 \sim 30\;{\rm{mHz}} $)內, 優于LISA pathfinder探測器的$ 3 \times {10^{ - 14}}\;{\rm{m}} \cdot {{\rm{s}}^{ - 2}} \cdot {\rm{H}}{{\rm{z}}^{ - \frac{1}{2}}} $技術指標[30].

                        圖  12  測試質量殘余加速度功率譜密度

                        Figure 12.  Power spectral density of the test mass's residual acceleration

                      • 本文設計了自抗擾控制律實現了噪聲、擾動、耦合情況下的無拖曳衛星本體姿態、衛星本體與測試質量間的相對位移以及相對姿態的聯合控制; 針對無拖曳子系統, 設計了測量飽和受限下的擴張狀態估計算法, 并 應用信息融合算法解決了應用“高階狀態敏感器”進行擴張狀態估計時存在的誤差累積問題; 并進行了仿真實驗.

                        接下來的首要工作就是建立本文所提算法的穩定性分析方法以及閉環系統的穩定性分析方法. 此外, 諸如有色測量噪聲的情形、系統執行機構存在延時的情形、執行器動作頻率與敏感器采樣頻率間的搭配問題等等 都是有意義的研究課題.

                    WeChat 關注分享

                    返回頂部

                    目錄

                      /

                      返回文章
                      返回