基于干扰力矩补偿的空间飞行器姿态控制方法

邓博炜，田 源，王 悦，金 文

（北京航天长征飞行器研究所，北京，100076）

0 引 言

空间飞行器在飞行过程中需要开启轨控发动机进行机动，其在提供所需过载的同时，由于安装误差等非理想化因素，也会产生附加的干扰力矩。其中影响较大的是飞行器质心漂移和轨控发动机推力偏心、推力偏移引起的干扰力矩，并且这几个误差项在地面试验中难以进行测量。若其引起的干扰力矩过大，可能导致姿态控制精度无法满足要求，不利于导引头跟踪目标。

在已经进行过的空间飞行器姿态控制研究中，多是将质心漂移、轨控推力偏心和偏移引起的干扰力矩与其他外部干扰力矩一起考虑，视作一个总的有界干扰，并进一步设计各种鲁棒控制律。但将其视为有界干扰，难免存在控制保守的问题。

有学者针对轨控干扰力矩的估计进行了一些相关研究。梁彤等考虑到轨控干扰力矩在地面试验中难以测量，提出了一种在飞行过程中进行的测试方法，在空间飞行器角速度较小时，令轨控发动机轮流开机，利用开机时刻和关机待惯组稳定后所测角速度来计算轨控干扰力矩，但其基于轨控发动机开机时引起的干扰力矩为常值的假设。张朋等分别基于扩展卡尔曼滤波（Extended Kalman Filter,EKF）和无迹卡尔曼滤波（Unscented Kalman Filter，UKF），推导了飞行器推力偏心和质心漂移的滤波模型并进行数学仿真，但只考虑了一个轨控发动机工作的情况，且采用飞行器的速度作为量测变量，通常认为飞行器速度是由卫星导航系统获得，因此该方法不适用于无卫星导航的情况。Tahk 等利用空间飞行器地面悬浮度试验的数据，选取角速度和视加速度作为量测变量，基于扩展卡尔曼滤波，对质心漂移和轨控推力偏心进行了估计，但同样只考虑了仅安装一个轨控发动机的情形。

本文考虑质心漂移、轨控推力偏心和推力偏移这几个引起干扰力矩的主要误差源，建立了相应的空间飞行器姿态动力学方程，选取惯组测得的角速度作为量测变量。通过可观测性秩条件，对该系统中状态变量的可观测性进行了分析。在可观测性分析的基础上，基于扩展卡尔曼滤波对空间飞行器轨控发动机引起的干扰力矩进行在线估计，并进一步在姿态控制中对其做出补偿。通过数学仿真，验证干扰力矩估计的可行性及其补偿对姿态控制精度的改善。

1 空间飞行器的动力学建模

1.1 空间飞行器的姿态动力学

本文针对的研究对象是一种典型布局的空间飞行器，4 个轨控发动机正交安装在飞行器标称质心所在的横截面上，6 个姿控发动机安装在飞行器的尾部，发动机布局及其编号如图1 所示。在现实中，由于结构、安装误差等因素的影响，飞行器的实际质心和设计的标称质心未必能够重合，并且随着燃料的消耗，实际质心的位置也会发生变化，称为质心漂移；发动机推力的实际方向未必与理想的推力作用线重合，称为推力偏心，此外，发动机推力的实际作用点位置与理想作用点也会存在一定的偏差，称为推力偏移。质心漂移、轨控推力偏心和推力偏移引起的干扰力矩是影响姿态控制精度的主要因素。

图1 发动机布局及编号Fig.1 Configuration and Serial Number of Thrusters

本文对空间飞行器进行建模时，认为其姿控发动机的推力偏心和推力偏移较小，故在姿态动力学模型中对其引起的干扰力矩忽略不计。考虑质心漂移、轨控发动机推力偏心和推力偏移时的姿态动力学模型如下：

式中J ,J ,J为飞行器沿载体系各轴的转动惯量；ω , ,为载体系中的飞行器角速度；M ,M ,为姿控发动机和轨控发动机的合力矩在载体系各轴的分量；,,为外部随机干扰力矩在载体系中的分量。

作用在空间飞行器上的合外力（除重力）为

式中F ,F ,F为飞行器所受合外力（除重力）在载体系各轴的分量；,,,为1#至4#轨控发动机的推力大小；,,,,,为1#至6#姿控发动机的推力大小； ,分别为1#轨控发动机真实推力方向与平面和平面的夹角；,分别为2#轨控发动机真实推力方向与平面和平面的夹角；,分别为3#轨控发动机真实推力方向与平面和平面的夹角；,分别为4#轨控发动机真实推力方向与平面和平面的夹角，即推力偏心。由这些推力偏心引起的轨控推力在体坐标轴上的分量为正时，相应的推力偏心值为正，反之为负。

进而空间飞行器受到的姿控发动机和轨控发动机的合力矩可以由下式计算：

式中为空间飞行器的半径； L为载体系原点（理想质心）到姿控发动机安装平面的距离；,,为飞行器真实质心在载体系中的坐标，即质心漂移；Δ, Δ,Δ为#（=1,2,3,4）轨控发动机真实作用点位置相对于理想作用点的偏差在载体系中的分量，即推力偏移。

1.2 滤波模型的建立

对式（3）进行近似处理，得到便于滤波的形式，在滤波时实际采用式（4）所示的近似合力矩：

式中 status为（#=1,2,3,4）轨控发动机状态，st atus=1表示#轨控发动机为开机状态，st atus= 0表示#轨控发动机为关机状态；F，F为合外力分量，可以由惯组测得的视加速度乘以空间飞行器质量得到。

由于质心漂移、轨控推力偏心等通常都是1 0数量级或更小的量，通过对式（3）和式（4）中各力和力矩的数量级进行分析可知，当轨控发动机等效推力远大于姿控发动机标称推力时，用式（4）所示的近似合力矩来代替式（3）所示的合力矩，能够达到很高的精度。

定义各轨控发动机的等效力臂为

用式（9）至式（16）定义的等效力臂对近似合力矩式（4）中的相应变量进行替换，则式（4）可以改写成：

选取惯组测得的飞行器角速度作为量测变量，选取飞行器角速度和各轨控发动机的等效力臂作为待估计的状态变量，可以建立估算轨控干扰力矩的状态方程和量测方程。

状态方程如下：

状态方程式（18）可以写成如下形式：

量测方程式（20）可以写成如下形式：

2 可观测性分析

在设计卡尔曼滤波器之前，通常先进行系统的可观测性分析。由于系统的可观测性与控制输入无关，并忽略状态噪声和量测噪声，则由式（19）和式（21）组成的系统可以写成：

记轨控干扰力矩：

新能源汽车的普及和发展离不开政府的技术和资金支持，国家财政需要充分发挥自身的积极作用，全面推动我国新能源汽车的快速进步。政府支持新能源汽车的发展和应用应体现在技术和财力方面，上述条件都能促进新能源汽车的发展。另外，国家还应对新能源汽车实施强有力的财政补贴和税收优惠政策，同时还要高度关注汽车产业体系的建设与发展。

显然式（24）表示的系统是一个非线性系统，其可观测矩阵为

如果对于定义区间内的任意，有()满秩，则称由式（24）表示的系统满足可观测性秩条件，进而该系统局部弱可观。

新状态向量的维数=6 ，对于式（24）表示的系统，有：

3 扩展卡尔曼滤波

显然由状态方程式（19）和量测方程式（21）组成的系统是一个非线性系统，为得到较高精度的轨控发动机等效力臂估计值，同时兼顾计算的快速性要求，本文选取基于滤波值线性化的方式，即扩展卡尔曼滤波。基于EKF 的滤波算法如下。

b）量测更新。

式中K为时刻的滤波增益矩阵；H为时刻的量测矩阵；R为量测噪声的方差阵；Z为时刻的量测向量。

对式（17）所示的近似合力矩进行分析。显然，当某个轨控发动机处于关闭状态时，其对应的等效力臂不可观；姿控发动机的实际推力无法实时测量，只能用其标称推力作为估计值，当姿控发动机推力处于建立或撤消阶段时，该估计的误差较大；由于轨控发动机等效力臂与姿控发动机力臂相比是小量，而姿控发动机推力与标称值存在偏差，当轨控发动机的等效推力较小时，其等效力臂估计受姿控推力偏差影响较大。综合以上因素考虑，本文仅在当影响某一轨控等效力臂估计的所有姿控发动机推力均处于稳定或关闭状态、轨控等效推力远大于姿控标称推力时，才对其进行滤波，否则关闭其滤波。而对载体角速度的滤波始终进行。关闭对某个状态变量滤波的具体方法是：在每一步滤波中，将误差协方差阵 P中代表该状态变量与其余状态变量估计误差协方差的非对角元素置为0。令代表该状态变量估计误差方差的对角元素保持不变，该状态变量的估计值也保持不变，直到重新开始对该状态变量滤波为止。

4 仿真分析

4.1 仿真条件

b）假设轨控发动机在给出开机指令10 ms 内推力进入稳态，其稳态值与标称推力存在10%的常值偏差，在稳态值基础上还存在幅值为5%的推力波动，在给出关机指令10 ms 内推力减少到0；姿控发动机在给出开机指令5 ms 内推力进入稳态，其稳态值与标称推力存在5%的常值偏差，在稳态值基础上还存在幅值为1%的推力波动，在给出关机指令5 ms 内推力减少到0。

c）在动力学模型中飞行器角速度的状态噪声方差为10rad/s，飞行器角速度的量测噪声方差为10rad/s。

d）制导律采用比例导引，姿态控制采用PID 控制，连续变化的制导和姿态控制指令通过PWPF 调节器离散化为常推力发动机的开关指令；

e）取姿态控制指令的计算周期为2 ms，姿控发动机开关指令的更新周期为10 ms，轨控发动机开关指令的更新周期为30 ms，扩展卡尔曼滤波的计算周期为1 ms；

f）待估状态变量的滤波初值均取为0，状态误差协方差阵的初值取为= 10⋅，状态噪声的方差阵取为Q= 10⋅，量测噪声的方差阵取为R= 10⋅；

h）认为姿控发动机的推力偏心较小，从而将其忽略，传感器误差只考虑量测噪声。

4.2 结果分析

当影响滚转通道的两个轨控发动机同时开机时，其等效力臂的估计存在耦合，而每个通道的轨控干扰力矩始终可观，此外对于姿态控制，主要就是关心轨控干扰力矩，因此在本小节中仅给出由等效力臂滤波值得到的轨控干扰力矩的估计结果和有/无轨控干扰力矩补偿时的姿态控制结果对比。

图2给出了滚转、偏航、俯仰通道的轨控干扰力矩估计值与真实值的对比。在考虑转动惯量变化和姿控发动机稳态推力偏差的情况下，虽然轨控干扰力矩的估计值与真实值存在一定偏差，但其估计精度还处于可以接受的范围，能够在较大程度上反映出真实轨控干扰力矩的大小和变化。特别是从图2a 中17.07~ 17.11 s 的曲线可以看出，当滚转通道同时存在2 个轨控发动机开机时，其总的轨控干扰力矩仍具有较好的估计效果，这也验证了本文第2 节中可观测性分析的正确性。

图2 轨控发动机干扰力矩的估计Fig.2 Estimation of Disturbance Torques of Divert Thrusters

分析式（18）所示的滤波状态方程，转动惯量的实际变化规律未知，姿控发动机的稳态推力也无法实时测量，只能采用其标称推力作为估计值。滤波模型和动力学模型中转动惯量和姿控稳态推力的差异导致了滤波结果的误差。

图3为有/无轨控干扰力矩补偿时俯仰通道的姿态控制效果对比。

图3 有/无轨控干扰力矩补偿的俯仰角Fig.3 Pitch Angle with/without Compensation of Disturbance Torque Caused by Divert Thrusters

由图3 可知，在22 s 后，补偿的效果十分显著，若不对轨控干扰力矩进行补偿，则俯仰角偏差迅速增大，最大偏差可以达到0.81°，而对其进行补偿则可以获得很高的俯仰控制精度。在5~22 s 的时间段内，有轨控干扰力矩补偿的俯仰控制精度也优于无补偿情况。图4 为有/无轨控干扰力矩补偿时偏航通道的姿态控制效果对比。

图4 有/无轨控干扰力矩补偿的偏航角Fig.4 Yaw Angle with/without Compensation of Disturbance Torque Caused by Divert Thrusters

有轨控干扰力矩补偿时最大偏航角偏差为0.54°，无轨控干扰力矩补偿时最大偏航角偏差为0.57°，2 种情况下的偏航角控制精度无显著区别。这是由于根据制导指令，安装在飞行器载体系轴方向的轨控发动机开机次数较少，相应地其引起的干扰力矩对偏航通道控制的影响较小，因此对其进行补偿的效果也不是很明显。

图5为有/无轨控干扰力矩补偿时滚转通道的姿态控制效果对比。与俯仰通道的情况类似，在22 s 以后，不进行轨控干扰力矩补偿的情况下，滚转角偏差逐渐增大，最大偏差达到1.24°；而对轨控干扰力矩进行补偿后，滚转角偏差保持在0.43°以内，干扰力矩补偿的效果显著。

图5 有/无轨控干扰力矩补偿的滚转角Fig.5 Roll Angle with/without Compensation of Disturbance Torque Caused by Divert Thrusters

5 结 论

本文基于EKF 对状态变量进行了估计，数学仿真结果表明，即使滤波模型中使用的转动惯量和姿控发动机稳态推力与动力学模型存在一定偏差，也能对轨控干扰力矩实现较好的估计。当滚转通道存在2 个轨控发动机同时开机时，其总的轨控干扰力矩仍具有较好的估计效果，这也验证了前述可观测性分析的正确性。对轨控干扰力矩进行补偿能够改善姿态控制效果，轨控发动机开机越频繁，补偿效果越显著。轨控干扰力矩的估计精度主要是受转动惯量和姿控发动机稳态推力这2 个误差源影响，姿控发动机稳态推力偏差可以通过发动机设计和工艺水平的提升来减小，而转动惯量的实际变化规律无法获得，但根据数学仿真的结果，若转动惯量在其初值附近较小的范围内变化，轨控干扰力矩的估计就能够达到较高的精度。