<sub id="bz1nl"></sub><sub id="bz1nl"></sub>
        <sub id="bz1nl"></sub>

        <sub id="bz1nl"></sub><sub id="bz1nl"></sub>

          2.793

          2018影响因子

          (CJCR)

          • 中文核心
          • EI
          • 中国科技核心
          • Scopus
          • CSCD
          • 英国科学文摘

          留言板

          尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

          姓名
          邮箱
          手机号码
          标题
          留言内容
          验证码

          饱和约束测量扩张状态滤波与无拖曳卫星位姿自抗扰控制

          杨飞 谈树萍 薛文超 郭金 赵延龙

          杨飞, 谈树萍, 薛文超, 郭金, 赵延龙. 饱和约束测量扩张状态滤波与无拖曳卫星位姿自抗扰控制. 自动化学报, 2020, 46(11): 2337?2349. doi: 10.16383/j.aas.c190515
          引用本文: 杨飞, 谈树萍, 薛文超, 郭金, 赵延龙. 饱和约束测量扩张状态滤波与无拖曳卫星位姿自抗扰控制. 自动化学报, 2020, 46(11): 2337?2349. 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, 46(11): 2337?2349. 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, 46(11): 2337?2349. 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

          • 基金项目:  国家重点研发计划(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 (2018YFA0703800) and National Natural Science Foundation of China (61773054, 61633003-3)
          • 摘要: 无拖曳卫星的本体姿态、卫星本体与测试质量间的相对位移及相对姿态的联合控制受到外部扰动、输入噪声、测量噪声及饱和约束、输入耦合以及状态耦合等因素的影响, 控制器的设计面临挑战. 本文采用基于扩张状态的卡尔曼滤波对系统状态和系统扰动进行实时估计, 引入自抗扰控制策略进行了控制器设计. 针对无拖曳控制子系统设计了测量饱和受限下的扩张状态估计算法, 并进行了信息融合. 在设计控制律时不仅考虑了对外部扰动的补偿, 还将系统状态间的耦合关系看成内部扰动进行补偿, 使得被控系统等价为“积分串联型系统”, 在此基础上实现了无拖曳卫星的联合控制. 数值仿真验证了方法的有效性和合理性.
          • 图  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[ \; 1\;\;0\;\;0 \; \right]$ $\left[\; 0\;\;1\;\;0 \;\right]$ $\left[ \; 0\;\;0\;\;1 \;\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} }{{I}_3}({\rm{kg} } \cdot { {\rm{m} }^{\rm{2} } })$
            ${I_{{\rm{sc}}}}$$\left[ {\begin{aligned}\;&{200}\;\;\;\;\;\;\;\;1\;\;\;\;\;\;\;\;\;2\\\;&\;\;1\;\;\;\;\;\;{2\;700}\;\;\;\;\;\;1\\ \;&\;\;2\;\;\;\;\;\;\;\;\; 1\;\;\; \;\;\;\;{2\;650} \end{aligned} } \right]({\rm{kg} } \cdot { {\rm{m} }^{\rm{2} } })$
            ${K_{{\rm{trans}}}}$$\left[ {\begin{aligned}\; & \;\;\;1\;\;\;\;\;\;\; {0.039}\;\;\;\;\;\; {0.039}\\\; &{0.039}\;\;\;\;\;\; \;1\;\;\;\;\;\;\;\;\; {0.039}\\\;&{0.039}\;\;\;\; {0.039}\;\;\;\; \;\;\;\;\;1 \end{aligned} } \right] \times {10^{ - 6} }{{\;({\rm{N}}/{\rm{m}})} }$
            ${D_{{\rm{trans}}}}$$1.4 \times {10^{ - 11} }{ {I}_3}\;({\rm{N} } \cdot {\rm{m} } \cdot {\rm{s/rad} })$
            ${K_{{\rm{rot}}}}$$\left[ {\begin{aligned} \;&\;1\;\;\;\;{10}\;\;{10}\\ \;&{10}\;\;\;\;1\;\;\;{10}\\\;&{10}\;\;\;{10}\;\;\;1 \end{aligned} } \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{aligned} { - {\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{aligned} } \right]({\rm{mN} } \cdot {\rm{m} })$
            ${{{F}}_{Dsc}}$$\left[ {\begin{aligned} { - {\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{aligned} } \right]({\rm{mN} })$
            ${{{T}}_{Dtm}}$0
            $\omega_d$$1.2 \times {10^{ - 3}}\;{\rm{Hz}}$
            下载: 导出CSV
            七星彩规则
          • [1] 罗子人, 钟敏, 边星, 董鹏, 董玉辉, 高伟, 等. 地球重力场空间探测: 回顾与展望. 力学进展, 2014, 44(1): 291?337

            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] 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 the 2005 IEEE Networking, Sensing and Control. IEEE, 2005: 728?733
            [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 the 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, China: 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

            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

            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

            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(9): 1725?1728

            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(9): 1725?1728
            [17] 刘昊, 魏承, 谭春林, 刘永健, 赵阳. 空间充气展开绳网系统捕获目标自抗扰控制研究. 自动化学报, 2019, 45(9): 1691?1700

            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(9): 1691?1700
            [18] 刘智勇, 何英姿. 相对位置和姿态动力学耦合航天器的自抗扰控制器设计. 航天控制, 2010, 28(2): 17?22

            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] 吴忠, 黄丽雅, 魏孔明, 郭雷. 航天器姿态稳定自抗扰控制. 第三十二届中国控制会议. IEEE, 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

            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] 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] 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] 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] 史大威, 杨肖, 蔡德恒, 牟治宇, 刘蔚, 纪立农. 基于胰岛素基础率估计的人工胰腺系统自抗扰控制[J]. 自动化学报, 2020, 46(): 1-15.
            [2] 武宪青, 徐可心, 张益波. 基于输出反馈的欠驱动TORA系统的有界输入控制[J]. 自动化学报, 2020, 46(1): 200-204. doi: 10.16383/j.aas.c180625
            [3] 刘昊, 魏承, 谭春林, 刘永健, 赵阳. 空间充气展开绳网系统捕获目标自抗扰控制研究[J]. 自动化学报, 2019, 45(9): 1691-1700. doi: 10.16383/j.aas.c180835
            [4] 李向阳, 哀薇, 田森平. 惯性串联系统的自抗扰控制[J]. 自动化学报, 2018, 44(3): 562-568. doi: 10.16383/j.aas.2018.c160568
            [5] 黄大荣, 柴彦冲, 赵玲, 孙国玺. 考虑多源不确定信息的路网交通拥堵状态辨识方法[J]. 自动化学报, 2018, 44(3): 533-544. doi: 10.16383/j.aas.2018.c160373
            [6] 程赟, 陈增强, 孙明玮, 孙青林. 多变量逆解耦自抗扰控制及其在精馏塔过程中的应用[J]. 自动化学报, 2017, 43(6): 1080-1088. doi: 10.16383/j.aas.2017.c170137
            [7] 刘晓东. 针对一类非线性系统的多变量线性 扩张状态观测器及其收敛性分析[J]. 自动化学报, 2016, 42(11): 1758-1764. doi: 10.16383/j.aas.2016.c150707
            [8] 李杰, 齐晓慧, 夏元清, 高志强. 线性/非线性自抗扰切换控制方法研究[J]. 自动化学报, 2016, 42(2): 202-212. doi: 10.16383/j.aas.2016.c150338
            [9] 陈增强, 孙明玮, 杨瑞光. 线性自抗扰控制器的稳定性研究[J]. 自动化学报, 2013, 39(5): 574-580. doi: 10.3724/SP.J.1004.2013.00574
            [10] 甄子洋, 王志胜, 王道波. 基于信息融合估计的离散线性系统预见控制[J]. 自动化学报, 2010, 36(2): 347-352. doi: 10.1360/SP.J.1004.2010.00347
            [11] 顾启泰, 方靖. 广义联邦滤波器的全局最优性[J]. 自动化学报, 2009, 35(10): 1310-1316. doi: 10.3724/SP.J.1004.2009.01310
            [12] 孙书利, 吕楠. 带有色观测噪声多传感器多重时滞系统分布式融合滤波器[J]. 自动化学报, 2009, 35(1): 46-53. doi: 10.3724/SP.J.1004.2009.00046
            [13] 甄子洋, 王志胜, 王道波. 基于信息融合最优估计的非线性离散系统预测控制[J]. 自动化学报, 2008, 34(3): 331-336. doi: 10.3724/SP.J.1004.2008.00331
            [14] 陈国栋, 贾培发. 基于扩张状态观测的机器人分散鲁棒跟踪控制[J]. 自动化学报, 2008, 34(7): 828-832. doi: 10.3724/SP.J.1004.2008.00828
            [15] 乔国林, 童朝南, 孙一康. 基于神经网络自抗扰控制的结晶器液位拉速协调系统研究[J]. 自动化学报, 2007, 33(6): 641-648. doi: 10.1360/aas-007-0641
            [16] 潘泉, 于昕, 程咏梅, 张洪才. 信息融合理论的基本方法与进展[J]. 自动化学报, 2003, 29(4): 599-615.
            [17] 张启忠, 蒋静坪. 用于机器人信息融合的RS智能系统[J]. 自动化学报, 2002, 28(5): 797-801.
            [18] 韩崇昭, 朱洪艳. 多传感信息融合与自动化[J]. 自动化学报, 2002, 28(增刊): 117-124.
            [19] 刘翔, 姜学智, 李东海, 李立勤, 胡雪蛟, 黎浩荣. 自抗扰控制器在过热汽温控制中应用的仿真研究[J]. 自动化学报, 2000, 26(增刊B): 187-191.
            [20] 邬永革, 杨静宇. 基于融合信息的道路分割方法[J]. 自动化学报, 1996, 22(5): 634-637.
          • 加载中
          图(12) / 表(2)
          计量
          • 文章访问数:  1872
          • HTML全文浏览量:  682
          • PDF下载量:  45
          • 被引次数: 0
          出版历程
          • 收稿日期:  2019-07-05
          • 录用日期:  2019-12-08
          • 网络出版日期:  2020-01-17
          • 刊出日期:  2020-11-20

          饱和约束测量扩张状态滤波与无拖曳卫星位姿自抗扰控制

          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

          摘要: 无拖曳卫星的本体姿态、卫星本体与测试质量间的相对位移及相对姿态的联合控制受到外部扰动、输入噪声、测量噪声及饱和约束、输入耦合以及状态耦合等因素的影响, 控制器的设计面临挑战. 本文采用基于扩张状态的卡尔曼滤波对系统状态和系统扰动进行实时估计, 引入自抗扰控制策略进行了控制器设计. 针对无拖曳控制子系统设计了测量饱和受限下的扩张状态估计算法, 并进行了信息融合. 在设计控制律时不仅考虑了对外部扰动的补偿, 还将系统状态间的耦合关系看成内部扰动进行补偿, 使得被控系统等价为“积分串联型系统”, 在此基础上实现了无拖曳卫星的联合控制. 数值仿真验证了方法的有效性和合理性.

          English Abstract

          杨飞, 谈树萍, 薛文超, 郭金, 赵延龙. 饱和约束测量扩张状态滤波与无拖曳卫星位姿自抗扰控制. 自动化学报, 2020, 46(11): 2337?2349. doi: 10.16383/j.aas.c190515
          引用本文: 杨飞, 谈树萍, 薛文超, 郭金, 赵延龙. 饱和约束测量扩张状态滤波与无拖曳卫星位姿自抗扰控制. 自动化学报, 2020, 46(11): 2337?2349. 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, 46(11): 2337?2349. 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, 46(11): 2337?2349. 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节介绍无拖曳卫星控制系统模型. 第2节介绍自抗扰控制器结构, 推导集值扩张状态滤波算法, 并进行信息融合, 进而设计了位姿联合控制律. 第3节给出了仿真实验. 第4节对本文进行了总结, 并对接下来需要进行的研究工作进行了简要介绍.

            • 无拖曳卫星的工作原理(见图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{{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}}}} = {[ {{{\varphi} _{{\rm{sc,pitch}}}}}\;\;{{\varphi _{{\rm{sc,yaw}}}}}\;\;{{\varphi _{{\rm{sc,roll}}}}} ]^{\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 = {[ {{{\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}} ]^{\rm T}}$${{Y}} \!\!=\!\![ {{{\varphi}} _{sc}^{\rm T}}$${{{{r}}_{{\rm{rel}}}^{\rm T}}\;\;{{{\varphi}}_{{\rm{rel}}}^{\rm T}} ]^{\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}}&{{{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}}&{{{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}}&{{{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{{{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}} {{{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{{I}_3}}&{{0_3}} \end{array}}\right], \qquad\qquad\qquad$$

              ${{u}} \!=\! [{{T}}{_{Csc}^{\rm{T}}}\; {{F}}{_{Csc}^{\rm{T}}}\; {{T}}{_{Ctm}^{\rm{T}}}]^{\rm T}$ 为期望的系统输入, ${{w}} \!=\! [{{w}}{_{{T_{Csc}}}^{\rm{T}}}\; {{w}}{_{{F_{Csc}}}^{\rm{T}}}\; {{w}}{_{{T_{Ctm}}}^{\rm{T}}}]^{\rm T}$为输入噪声, ${{f}} = [{{T}}{_{Dsc}^{\rm{T}}}\;$${{F}}{_{Dsc}^{\rm{T}}}\; {{T}}{_{Dtm}^{\rm{T}}}]^{\rm T}$为系统受到的外部扰动, 其标称模型$ \bar{{{f}}} $已知; $ {{d}} $为输出测量噪声. 这里及以后, ${I}_{3}$表示$ 3\times 3 $的单位阵, $ 0_{m} $表示元素全为0的$ m\times m $矩阵.

              注 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}}&{{{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{{I}_3}}&{{0_3}}&{{0_3}}&{{0_3}}\\ {{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{0_3}}&{{{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形式的递推实现对系统扩张状态的估计:

              假设 1. ($ {A_d},{C_d} $)可观测.

              假设 2. $ \{ {{{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}} $.

              假设 3. $ {\rm E}( {[ {{{{X}}_0} - {{\widehat {{X}}}_0}} ]{{[ {{{{X}}_0} -{{\widehat {{X}}}_0}}]^{\rm T}}}} ) \le {P_0} $, 其中$ {\widehat {{X}}}_0 $是对$ {{{{X}}_0}} $的估计值, $ {P_0} $为已知的常数矩阵.

              假设 4. $ {\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.

              算法 12. 基于扩张状态的卡尔曼滤波[23]

              初始化:

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

              时间更新:

              $$\small \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} $$

              其中 $\small \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}} $的估计值, 取值如下

              $$ \small \begin{split} & {{ {\hat{G}}}_{k-1,i}}= \\ & \begin{cases} \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{cases} \end{split} $$ (9)

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

              测量更新:

              $$ \small\begin{split} {\hat{X}}_k =& {\hat{X}}_k^ - + {K_k}({{{Y}}_k} - {C_d} {\hat{X}}_k^ -),\\ P_k =& ({I} - {K_k}{C_d})P_k^-{({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} $$

              上述算法中, ${I}$表示适当维数的单位矩阵; $ {\rm {trace}}(\cdot) $表示矩阵的迹(主对角线上各个元素的总和); ${\rm{diag}}\{{a_1},\cdots ,{a_n}\}$表示对角线元素为$ a_1,\cdots,a_n $的对角矩阵.

              注 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} = {[ {{r}_{{\rm{rel}}}}\;\;{{\dot r}_{{\rm{rel}}}} ]^{\rm{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{{{{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}} = {[ {{r}_{{\rm{rel}}}}\;\;{{\dot r}_{{\rm{rel}}}}\;\;f ]^{\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 = \bigg[ \begin{aligned} {{{A_o}}\;\;{{{B}}_o}}\\ {0\quad0\;\;} \end{aligned}\bigg]$, ${{B}} = \bigg[ \;\;\begin{aligned} {{{{B}}_o}}\\ {0\;} \end{aligned}\;\; \bigg]$, $ {{{B}}_e} = [0\; \; 0\; \; 1]^{\rm T} $, ${{C}} = [ {{{{C}}_o}}\;\;D_o ]$, $ 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所示):

              $$ {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 $为测量范围上限.

              图  3  饱和约束测量

              Figure 3.  Saturation-constrainted observations

              定义 1. $ Y_c^k = \{ {y_{c,1}},{y_{c,2}}, \cdots, {y_{c,k}}\}, $ 即用$ {Y_c^k} $表示传感器在$ k $次测量中得到的所有信息. 在第2.1节的假设1 ~ 4下, 系统的时间更新可以写为:

              $$ \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^ * = ({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}] = \\ &({I} - {{{K}}_k}{{{C}}}){\rm E}[{{{e}}_k^ -({{e}}_k^ - )^{\rm T}}|Y_c^{k}]{({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 \\ & ({I} - {{{K}}_k}{{{C}}})P_k^ -{({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^ * \; = & \; \; ({I} - {{{K}}_k}{{{C}}})P_k^ -{({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 \begin{cases}{\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}, \qquad\qquad\qquad{\text{其他}} \end{cases} \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}){\rm{d}}} {y_k}}}{{\int_{{a_k}}^{{b_k}} {\varphi ({y_k}|Y_c^{k - 1}){\rm{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} $.

              时间更新:

              $$\small \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)给出.

              测量更新:

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

              $$ \small {\hat{X}}_k = {\hat{X}}_k^ - + {{{K}}_k}({y_k} - {{C}} {\hat{X}}_k^ - -Du_k) \quad$$ (31)
              $$ \small\begin{split} P_k =& P_k^* = ({I} - {{{K}}_k}{{{C}}})P_k^-{({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)

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

              $$ \small {{ {\hat{X}}}_k} = {\hat{X}}_k^ -+{{{K}}_k}({\rm E}[{y_k}|Y_c^{k}]- {{{C}}} {\hat{X}}_k^ - -Du_k)\; \; \; \; \; \; \; $$ (33)
              $$ \small 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)给出.

              注 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{aligned} {{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{aligned}} \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[ \; 1\;\;0\;\;0 \; \right]$ $\left[\; 0\;\;1\;\;0 \;\right]$ $\left[ \; 0\;\;0\;\;1 \;\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) 运行算法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} $.

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

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

              其中加权系数$ \Lambda_1 $$ \Lambda_2 $由式(37)给出.

              3) 把融合后的结果返给算法1和算法2, 具体地有,

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

              其中, 上标 “$^+$” 表示一个矩阵的Moore-Penrose广义逆.

              注 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{{I}_3}{{{m_{{\rm{sc}}}}}}( {{{{F}}_{Csc}} + {{{{w}}_{{F_{Csc}}}}} + {{{F}}_{Dsc}}}) ={{{h}}_{idf}} - \\ &\dfrac{{{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{{{I}_3}}{{{m_{{\rm{sc}}}}}}( {{m_{{\rm{sc}}}}{{ {\hat{h}}}_{idf}} - {{{F}}_{Csc0}}} ) \approx\\ &\dfrac{{{{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中微分环节的的作用即可以降低该影响, 这样就实现了对相对姿态的调节.

              注 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} }{{I}_3}({\rm{kg} } \cdot { {\rm{m} }^{\rm{2} } })$
              ${I_{{\rm{sc}}}}$$\left[ {\begin{aligned}\;&{200}\;\;\;\;\;\;\;\;1\;\;\;\;\;\;\;\;\;2\\\;&\;\;1\;\;\;\;\;\;{2\;700}\;\;\;\;\;\;1\\ \;&\;\;2\;\;\;\;\;\;\;\;\; 1\;\;\; \;\;\;\;{2\;650} \end{aligned} } \right]({\rm{kg} } \cdot { {\rm{m} }^{\rm{2} } })$
              ${K_{{\rm{trans}}}}$$\left[ {\begin{aligned}\; & \;\;\;1\;\;\;\;\;\;\; {0.039}\;\;\;\;\;\; {0.039}\\\; &{0.039}\;\;\;\;\;\; \;1\;\;\;\;\;\;\;\;\; {0.039}\\\;&{0.039}\;\;\;\; {0.039}\;\;\;\; \;\;\;\;\;1 \end{aligned} } \right] \times {10^{ - 6} }{{\;({\rm{N}}/{\rm{m}})} }$
              ${D_{{\rm{trans}}}}$$1.4 \times {10^{ - 11} }{ {I}_3}\;({\rm{N} } \cdot {\rm{m} } \cdot {\rm{s/rad} })$
              ${K_{{\rm{rot}}}}$$\left[ {\begin{aligned} \;&\;1\;\;\;\;{10}\;\;{10}\\ \;&{10}\;\;\;\;1\;\;\;{10}\\\;&{10}\;\;\;{10}\;\;\;1 \end{aligned} } \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{aligned} { - {\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{aligned} } \right]({\rm{mN} } \cdot {\rm{m} })$
              ${{{F}}_{Dsc}}$$\left[ {\begin{aligned} { - {\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{aligned} } \right]({\rm{mN} })$
              ${{{T}}_{Dtm}}$0
              $\omega_d$$1.2 \times {10^{ - 3}}\;{\rm{Hz}}$

              敏感器采样以及执行器执行周期$h = {\rm{0.1\;s}}$, 输入噪声$ {{{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}}$. 假设4中的$ {{{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\nolimits_{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\nolimits_{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 \{ {{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\;{\rm{mHz}} \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

            • 本文设计了自抗扰控制律实现了噪声、扰动、耦合情况下的无拖曳卫星本体姿态、卫星本体与测试质量间的相对位移以及相对姿态的联合控制; 针对无拖曳子系统, 设计了测量饱和受限下的扩张状态估计算法, 并应用信息融合算法解决了应用“高阶状态敏感器” 进行扩张状态估计时存在的误差累积问题; 并进行了仿真实验.

              接下来的首要工作就是建立本文所提算法的稳定性分析方法以及闭环系统的稳定性分析方法. 此外, 诸如有色测量噪声的情形、系统执行机构存在延时的情形、执行器动作频率与敏感器采样频率间的搭配问题等都是有意义的研究课题.

          参考文献 (31)

          目录

            /

            返回文章
            返回