作为一种基于模型的递归滤波器，扩展卡尔曼滤波(EKF)利用一阶线性化的方法对卡尔曼滤波(KF)进行扩展，是当前较为成熟和使用广泛的状态/参数估计方法[5]。虽然EKF可以通过将参数扩展为状态量的方式对参数进行估计，但受限于噪声模型难以估计时变参数；此外，EKF也无法处理状态量的约束[6]。而滚动时域估计(MHE)方法则从最优控制问题的角度出发[7]，并引入滚动时域策略[8]，通过求解优化问题实现对状态/参数的估计；因此滚动时域估计能够较好地处理带约束的估计问题和状态/参数联合估计问题[5]，近年来得到越来越广泛的应用。文献[9]建立了滚动时域估计渐近稳定和有界稳定的充分条件，并给出了一种算法实现。文献[10]在化学工程问题中利用滚动时域估计进行状态/参数联合估计。文献[11]将滚动时域估计用于航天器的姿态估计和传感器参数校正。文献[12]则研究了机器人的多速率采样滚动时域估计问题。
对滚动时域估计的研究可分为两大类：到达代价的计算和优化问题的求解。到达代价的计算直接关系到滚动时域估计的准确性和稳定性。文献[7]推导了滚动时域估计稳定的充分条件，提出利用估计的协方差矩阵计算到达代价，这也是目前计算到达代价的主流思路。基于文献[7]的研究成果，文献[13]采用卡尔曼滤波的误差协方差矩阵计算到达代价，文献[14-15]利用EKF计算到达代价，文献[16]则利用无迹卡尔曼滤波(UKF)计算到达代价。
这些方法都是从概率与统计的角度出发，且并未利用滚动时域估计的结果，因此是一类开环的到达代价计算方法，不利于进一步提高估计精度。针对这一问题，本文从动态规划原理出发，将到达代价的计算转化为最小二乘问题，并利用QR分解给出计算方法。该方法在到达代价的计算过程中使用了最新估计值，因而形成了反馈机制，有利于提高估计精度与速度。
1 弹性高速飞行器数学模型 本文主要研究弹性高速飞行器的纵向状态/参数估计问题，因此使用如下纵向动力学模型[17]：
 | (1) |
式中:m为飞行器质量；V为飞行器速度大小；θ为弹道倾角；L为升力；g为重力加速度；
本文考虑2种传感器布置方案。方案1采用常规的传感器输出：角速度信号ωm和过载信号nm。
 | (2) |
式中: $φ$i(x)为第i阶弹性模态的振型；xs为传感器的轴向坐标；nz为刚体模态产生的过载。
一般地，弹性高速飞行器仅一阶弹性模态与刚体运动及控制系统频带耦合[17-18]，因此本文仅考虑一阶模态。忽略长周期模态，且考虑弹性模态固有频率的变化，令
 | (3) |
式中:δ为飞行器的舵偏角。
方案2采用分别安装在弹体前半部(距头部xs1)和后半部(距头部xs2)的2套陀螺[4]：
 | (4) |
分别将上述信号作差，可得到与刚体运动无关的角速度信号分量ωe和受到弹性振动信号扰动的角速度信号分量ωr：
 | (5) |
同样可令
 | (6) |
综上，弹性高速飞行器的数学模型如下：
 | (7) |
2 基于QR分解的滚动时域估计 2.1 问题描述 对于式(7)所示的连续系统，可利用差分、多重打靶等方法转化为式(8)所示的离散系统：
 | (8) |
式中：wk和vk分别为系统噪声序列和量测噪声序列，通常情况下假设二者服从零均值高斯分布：
 | (9) |
其中:Q和R分别为系统噪声和量测噪声的协方差矩阵。
系统式(8)的状态估计问题可转化为如下优化问题：
 | (10) |
式中：
 | (11) |
其中: T为当前时刻；
优化问题式(10)利用了全部的测量数据，因此称全信息估计。全信息估计的计算量会随T的增长而迅速增大到无法接受的地步，因此引入滚动时域策略以限制全信息估计的维数，形成滚动时域估计方法。滚动时域估计方法的目标函数ΦT变为
 | (12) |
式中：CT-N(xT-N)为到达代价，根据前向动态规划原理，其定义如下：
 | (13) |
其中:x(τ, x0, {wk})表示在初值为x0且受到噪声序列{wk}的情况下τ时刻状态量的取值。
由式(13)可知，到达代价是一个表达式非常复杂的函数[19]。因此，一般用如下二次函数近似表示到达代价：
 | (14) |
式中:ΦT-N*为T-N时刻优化指标的最优值；xT-N为T-1时刻对T-N时刻状态的估计；PT-N为估计误差的权重矩阵。
由于ΦT-N*为常数，因此计算到达代价实际上就是计算xT-N和PT-N。目前多数文献多采用卡尔曼滤波及其误差协方差矩阵更新公式计算到达代价，但该方法未利用滚动时域估计的结果，不利于估计精度和速度的提高。本文从优化问题求解的角度出发，给出了一种基于QR分解的到达代价计算方法。
2.2 基于QR分解的到达代价计算方法 在T+1时刻，有
 | (15) |
因此，利用式(14)所示函数估计到达代价，根据前向动态规划原理可得
 | (16) |
对权重矩阵PT-N、R-1和Q-1作Cholesky分解，有
 | (17) |
利用式(17)，式(16)可写为
 | (18) |
式中:‖z‖22表示zTz。
为得到式(18)所示最优化问题的解析解，在滚动时域估计得到的最优估计xT-N*处对非线性函数F和H线性化：
 | (19) |
式中：
 | (20) |
同理可得
 | (21) |
式中：
 | (22) |
将式(19)和式(21)代入式(18)可得
 | (23) |
令
 | (24) |
式(23)所示的到达代价计算问题转换为如下所示的最小二乘问题：
 | (25) |
为求解式(25)所示的最小二乘问题，对A作QR分解。
 | (26) |
式中:L为正交矩阵；R1和R2为上三角矩阵；R12为矩阵。
此外为表示方便，设
 | (27) |
将式(26)和式(27)代入式(25)可得
 | (28) |
式(28)中的变量仅有xT-N，因此该最小二乘问题的解为
 | (29) |
将式(29)代入式(28)可得
 | (30) |
与式(14)比较即可得到到达代价的更新方程:
 | (31) |
综上，到达代价的更新计算方法如下：
步骤1??分别对权重矩阵PT-N、R-1和Q-1作Cholesky分解，得到PT-N、R-1和Q-1。
步骤2??线性化函数F和H。
 |
步骤3??构造矩阵A和b。
 |
步骤4??对A进行QR分解。
 |
并计算
 |
步骤5??计算xT-N+1和PT-N+1。
 |
即完成对到达代价的更新。
2.3 滚动时域估计问题的求解 由2.1节和2.2节可知，弹性高速飞行器的状态估计问题转化为固定维数的优化问题。但这需要对系统状态和输入进行采样，因此对于弹性高速飞行器这一连续系统，需要选用合适方法完成系统方程的离散。
考虑到系统非线性较强且采样速率恒定，因此选用离散节点间距恒定且精度较高的多重打靶法。多重打靶法在等间距的离散节点上对状态量和控制量进行离散，认为相邻节点间控制量恒定，并加入节点处状态量相等的约束，从而实现连续系统的离散化，最终将状态估计问题转化为式(32)所示的非线性规划问题。
 | (32) |
本文采用较为成熟的序列二次规划方法求解非线性规划问题式(32)。
综上，滚动时域估计方法的步骤如下：
步骤1??初始化。给定P0、R-1、Q-1，初始状态估计x0和滚动时域窗口长度N。
步骤2??当k < N时，利用EKF更新公式计算状态估计值xk和协方差矩阵Pk。
步骤3??当k≥N时，利用序列二次规划方法求解非线性规划问题式(32)。
步骤4??利用2.2节中给出的到达代价更新策略计算xT-N和PT-N。
步骤5??在k+1时刻，获得量测yk+1，构造新的量测数据集[yk-N+2??…??yk??yk+1]，返回步骤3。
3 仿真分析 设窗口长度N=15，采样周期Δt=15 s，量测噪声协方差矩阵为R=diag([2.5×10-5??2.5×10-5])，系统噪声协方差矩阵为Q=diag([1×10-6??1×10-2??1×10-6??2.5×10-3??0.36])。系统输入信号如图 1所示，为了验证不同输入下方法的性能，在前25 s为正弦信号，后25 s无输入。此外，为了验证滚动时域估计方法对参数的估计性能，对一阶弹性模态固有频率加入了正弦扰动信号。
 |
| 图 1 输入信号 Fig. 1 Input signal |
| 图选项 |
在量测数据和估计初值相同的情况下，分别采用EKF、EKF更新到达代价的滚动时域估计(MHE-EKF)和QR分解更新到达代价的滚动时域估计(MHE-QR)对弹性高速飞行器的状态和一阶弹性模态的固有频率进行估计。
在传感器按照方案1布置的情况下，分别对上述3种方法进行100次Monte Carlo仿真。估计结果的均方根误差(Root Mean Square Error, RMSE)如图 2所示。
 |
| 图 2 EKF、MHE-EKF和MHE-QR方法估计结果的均方根误差 Fig. 2 RMSE of estimation results of EKF, MHE-EKF and MHE-QR methods |
| 图选项 |
由图 2(a)~(d)可知，2种滚动时域估计方法的均方根误差均明显小于EKF；且受益于优化的思路，滚动时域估计方法的收敛速度快于EKF。由图 2(e)可知，在系统输入变为0以后，EKF对弹性模态频率的估计结果出现了发散现象，并导致弹性模态的均方根误差大幅波动；而滚动时域估计并未受到系统输入的影响。以上结果均说明滚动时域估计方法的性能优于EKF。此外，由图 2(d)可知，EKF更新到达代价的情况下，
利用MHE-QR方法分别在传感器布置方案1(MHE-QR1)和传感器布置方案2(MHE-QR2)下进行100次Monte Carlo仿真，估计结果的均方根误差如图 3所示。
 |
| 图 3 不同方案时MHE-QR方法估计结果的均方根误差 Fig. 3 RMSE of estimation results of different schemes using MHE-QR method |
| 图选项 |
由图 3(a)、(b)可知，对于
表 1为上述方法的均方根误差均值。为验证估计弹性模态频率的必要性，表 1还显示了只估计状态量的滚动时域估计方法(MHE-S)和EKF(EKF-S)的均方根误差均值。
表 1 不同方法估计结果的均方根误差均值 Table 1 Average RMSE mean values of estimation results of different methods
| 方法 | RMSE | ||||
| / (10-3(°)) | ωz/(10-3(°)· s-1) | η1 | 1 | ω1/ (rad·s-1) | |
| MHE-QR1 | 0.536 | 3.4 | 0.037 7 | 0.15 | 0.87 |
| MHE-QR2 | 0.472 | 3.3 | 0.029 6 | 0.097 | 0.62 |
| MHE-EKF | 0.548 | 3.4 | 0.039 0 | 0.17 | 0.90 |
| EKF | 1.2 | 5.2 | 0.088 3 | 0.47 | 2.86 |
| MHE-S | 3.2 | 5.7 | 0.39 | 1.41 | |
| EKF-S | 3.5 | 7.0 | 0.37 | 2.47 | |
表选项
由表 1可知，MHE-QR和MHE-EKF的精度明显高于EKF。且MHE-QR对ω1的估计精度最高，MHE-EKF的估计误差大于MHE-QR，而EKF对ω1的估计出现了较大的误差。这是由于在滚动时域估计中，ω1只是一个优化变量，而噪声项只是提供了改变ω1取值的手段，对ω1的估计并不依赖具体的模型；而EKF将ω1看作状态量，对其的估计精度依赖于模型的准确度，但这类参数的变化并不存在具体的模型，因此EKF很难准确估计时变的参数。此外，只估计状态量的MHE-S和EKF-S的误差均明显大于其他3种同时估计参数和状态量的方法，说明对变化的参数进行估计是十分必要的。
表 2显示了3种方法的计算耗时，仿真在Windows 10系统(CPU为i5-7400，主频为3.00 GHz)中MATLAB R2017a环境下完成。EKF的耗时明显短于MHE-QR和MHE-EKF。MHE-QR的平均计算耗时短于MHE-EKF，且MHE-QR的最大计算耗时低于采样周期，具备应用潜力；而MHE-EKF虽然平均计算耗时低于采样周期，但最大计算耗时高于采样周期，表明EKF更新到达代价在计算效率上不如QR分解。
表 2 不同方法的计算耗时 Table 2 Run time of different methods
| 方法 | 平均时间/(10-2 s) | 最大时间/(10-2 s) |
| MHE-QR1 | 2.44 | 4.74 |
| MHE-QR2 | 2.35 | 4.78 |
| MHE-EKF | 2.48 | 7.56 |
| EKF | 0.66 | 1.27 |
表选项
4 结论 本文提出了一种基于QR分解的到达代价更新方法，并将其用于弹性高速飞行器的滚动时域估计中，实现了状态/参数联合估计。
1) 状态/参数联合估计方法的精度远高于只估计状态的方法。由于弹性高速飞行器弹性模态的固有频率并非常数，会随飞行器状态变化而变化，因此对其进行在线估计是非常必要的，能够有效提高状态估计的精度。
2) 滚动时域估计的精度明显高于EKF。相较于传统的EKF更新方法，QR分解更新到达代价在精度类似的同时，具有更快的计算速度(最大计算耗时优于EKF)。这得益于QR分解更新到达代价的策略利用了滚动时域估计的结果，形成了反馈机制，并通过直接求解优化问题更新到达代价。
3) 传感器采用布置方案2时的滚动时域估计结果好于布置方案1。这是由于方案2通过引入有效信息预估而进一步提升了估计效果。
4) QR分解更新到达代价的滚动时域估计方法的最长计算耗时低于采样速率，具有实际应用的潜力，后续应继续研究更快的优化算法，提高计算速度。
参考文献
| [1] | 张超凡, 宗群, 董琦, 等. 高超声速飞行器模型及控制若干问题综述[J]. 信息与控制, 2017, 46(1): 90-102. ZHANG C F, ZONG Q, DONG Q, et al. A survey of models and control problems of hypersonic vehicles[J]. Information and Control, 2017, 46(1): 90-102. (in Chinese) |
| [2] | 张伸, 王青, 董朝阳, 等. 基于跟踪微分器的高超声速飞行器减步控制[J]. 北京航空航天大学学报, 2017, 43(10): 2054-2062. ZHANG S, WANG Q, DONG C Y, et al. Reduced step control of hypersonic vehicle based on tracking differentiator[J]. Journal of Beijing University of Aeronautics and Astronautics, 2017, 43(10): 2054-2062. (in Chinese) |
| [3] | 赵贺伟, 杨秀霞, 沈如松, 等. 弹性高超声速飞行器预设性能精细姿态控制[J]. 兵工学报, 2017, 38(3): 501-511. ZHAO H W, YANG X X, SHEN R S, et al. Prescribed perfor-mance fine attitude control for aeroelastic hypersonic vehicle[J]. Acta Armamentarii, 2017, 38(3): 501-511. DOI:10.3969/j.issn.1000-1093.2017.03.012 (in Chinese) |
| [4] | 吴云洁, 宋嘉赟, 刘晓东, 等. 推力矢量防空导弹伺服弹性的抑制[J]. 北京航空航天大学学报, 2013, 39(11): 1480-1485. WU Y J, SONG J Y, LIU X D, et al. Suppression of aeroservoelasticity in anti-aircraft missile using thrust vector[J]. Journal of Beijing University of Aeronautics and Astronautics, 2013, 39(11): 1480-1485. (in Chinese) |
| [5] | ABDOLLAHPOURI M, TAKACS G, ROHAL-ILKIV B. Real-time moving horizon estimation for a vibrating active cantilever[J]. Mechanical Systems and Signal Processing, 2017, 86(A): 1-15. |
| [6] | 焦志强, 李卫华, 王鹏. 基于多模型与滚动时域估计的机动目标跟踪算法[J]. 空军工程大学学报(自然科学版), 2016, 17(2): 15-20. JIAO Z Q, LI W H, WANG P. A multi-model method of tracking maneuvering target based on multiple model and moving horizon estimation[J]. Journal of Air Force Engineering University(Natural Science Edition), 2016, 17(2): 15-20. DOI:10.3969/j.issn.1009-3516.2016.02.004 (in Chinese) |
| [7] | RAO C V, RAWLINGS J B, LEE J H. Constrained linear state estimation-A moving horizon approach[J]. Automatica, 2001, 37(10): 1619-1628. DOI:10.1016/S0005-1098(01)00115-7 |
| [8] | 陈伟, 孙传杰, 冯高鹏, 等. 基于滚动时域优化的旋转弹解耦控制器设计[J]. 北京航空航天大学学报, 2018, 44(4): 717-724. CHEN W, SUN C J, FENG G P, et al. Design of decoupling controller for spinning missile based on receding horizon optimal[J]. Journal of Beijing University of Aeronautics and Astronautics, 2018, 44(4): 717-724. (in Chinese) |
| [9] | RAO C V, RAWLINGS J B, MAYNE D Q. Constrained state estimation for nonlinear discrete-time systems:Stability and moving horizon approximations[J]. IEEE Transactions on Automa-tic Control, 2003, 48(2): 246-258. DOI:10.1109/TAC.2002.808470 |
| [10] | KUHL P, DIEHL M, KRAUS T, et al. A real-time algorithm for moving horizon state and parameter estimation[J]. Computers and Chemical Engineering, 2011, 35(1): 71-83. |
| [11] | VAN DER STEEN J, DIEHL M, AERTS C, et al. Spacecraft attitude estimation and sensor calibration using moving horizon estimation[J]. Journal of Guidance, Control, and Dynamics, 2013, 36(3): 734-742. |
| [12] | LIU A D, ZHANG W A, CHEN Z Q, et al. Moving horizon estimation for mobile robots with multirate sampling[J]. IEEE Transactions on Industrial Electronics, 2017, 64(2): 1457-1467. DOI:10.1109/TIE.2016.2611458 |
| [13] | 焦志强, 李卫华, 王鹏. 基于量测补偿的多传感器分布式滚动时域估计[J]. 系统工程与电子技术, 2017, 39(5): 984-990. JIAO Z Q, LI W H, WANG P. Distributed moving horizon estimation for multi-sensors system based on measurements compensation[J]. System Engineering and Electronics, 2017, 39(5): 984-990. (in Chinese) |
| [14] | GAO W, YANG J, LIU J, et al. Moving horizon estimation for cooperative localization with communication delay[J]. The Journal of Navigation, 2015, 68(3): 493-510. |
| [15] | 赵国荣, 黄婧丽, 苏艳琴, 等. 基于滚动时域估计的飞行器姿态估计及三轴磁强计在线校正[J]. 物理学报, 2015, 64(21): 210502. ZHAO G R, HUANG J L, SU Y Q, et al. Attitude estimation and three-axis magnetometer on-line calibration based on moving horizon estimation[J]. Acta Physica Sinica, 2015, 64(21): 210502. DOI:10.7498/aps.64.210502 (in Chinese) |
| [16] | QU C C, HAHN J. Computation of arrival cost for moving horizon estimation via unscented Kalman filtering[J]. Journal of Process Control, 2009, 19(2): 358-363. DOI:10.1016/j.jprocont.2008.04.005 |
| [17] | 孟中杰, 闫杰. 高超声速弹性飞行器振动模态自适应抑制技术[J]. 宇航学报, 2011, 32(10): 2164-2168. MENG Z J, YAN J. Adaptive modal suppression for hypersonic aeroelastic vehicler[J]. Journal of Astronautics, 2011, 32(10): 2164-2168. DOI:10.3873/j.issn.1000-1328.2011.10.011 (in Chinese) |
| [18] | FIORENTINI L, SERRANI A, BOLENDER M A, et al. Nonlinear robust adaptive control of flexible air-breathing hypersonic vehicles[J]. Journal of Guidance, Control, and Dynamics, 2009, 32(2): 401-416. |
| [19] | SANCHEZ Z, MURILLO M, GIOVANINI L. Adaptive arrival cost update for improving moving horizon estimation performance[J]. ISA Transactions, 2017, 68: 54-62. DOI:10.1016/j.isatra.2017.02.012 |
