CN106354147B - 一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统与方法 - Google Patents

Abstract

本发明涉及一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统与方法，包括：质心估计模块、预摆角计算模块、程序角去噪与补偿模块、控制反馈偏差计算模块、增量式PID控制模块、等效伺服摆角指令与限幅模块。本发明解决了在轨飞行器大质心横移姿轨耦合的高精度控制问题，达到了很好的工程应用效果。

Description

一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统 与方法

技术领域

本发明涉及一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统与方法，属于在轨飞行器控制技术领域。

背景技术

轨道运输飞行器，在轨道部署卫星分离的过程中，存在大质量偏移的情况，如某任务存在质心横移45mm，则干扰力矩达200N·m以上，若只利用姿控发动机控制，控制力矩为68N·m，目前姿控发动机控制能力不能满足控制要求，需要轨控发动机具备摇摆的能力。飞行过程中需要进行多次大范围轨道转移，飞行器主发动机为轨道控制和姿态控制合用，姿态控制和轨道控制相互影响，造成控制系统性能恶化，在发动机等大干扰下需要保持精确的姿态控制。因此需要研究姿态耦合控制技术实现控制系统的姿态快速跟踪及稳定。

发明内容

本发明要解决的技术问题是：克服现有技术的不足，提供一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统与方法，充分考虑在轨飞行器质心横移对控制系统影响，解决质心横移对起控特性影响，高频噪声对控制的影响，高精度跟踪控制等问题，确保飞行器高精度控制。

本发明技术解决方案：

一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统，包括：质心估计模块、预摆角计算模块、程序角去噪与补偿模块、控制反馈偏差计算模块、增量式PID控制模块、等效伺服摆角指令及限幅模块；

质心估计模块，根据控制计算机发送姿控发动控制指令，采集敏感器信息，通过估计方法，实时估计飞行器质心参数，然后传递给预摆角计算模块；

预摆角计算模块，根据质心估计模块实时估计飞行器质心参数计算出发动机预摆角及程序角补偿值，将程序角补偿值传递给程序角去噪与补偿模块对程序角进行补偿，将发动机预摆角发送给等效伺服摆角指令及限幅模块，对伺服摆角进行预摆处理；

程序角去噪与补偿模块，对制导计算模块输出的程序角指令进行滤波处理，然后根据预摆角计算模块输出程序角补偿值，将制导输出程序角由本体系旋转到质心系，将补偿后程序角输出给控制反馈偏差计算模块；

控制反馈偏差计算模块，输入为导航模块计算飞行器当前相对于轨道系姿态信息和由程序角去噪与补偿模块输出的程序角，然后进行四元数及姿态角速度偏差求解，并将偏差值输出给增量式PID控制模块；

增量式PID控制模块，根据控制反馈偏差计算模块输出的四元数偏差和姿态角速度偏差，采用增量式PID控制律计算出俯仰、偏航通道摆角控制指令，将摆角控制指令传递给等效伺服摆角指令及限幅模块；

等效伺服摆角指令计算及限幅模块，接收增量式PID控制模块输出的俯仰、偏航通道摆角控制指令以及预摆角计算模块输出的发动机预摆角指令，将两个指令信号进行叠加，根据伺服安装位置，将摆角指令分配为伺服电机I、II控制指令，并根据发动机摇摆范围，对伺服电机控制指令进行限幅，并将限幅后的伺服电机控制指令发送给综合控制器。

优选的，质心估计模块实现过程如下：

控制计算机分别对飞行器三通道正负方向发送定时姿控发动机开关指令，假定姿控发动机推力及力臂已知，即控制力矩系数已知，给定转动动量初值，由敏感器测量姿态角速度及姿态角加速度信息，从这些数据建立仿射系统本体属性的数学模型，采用带遗忘因子递推最小二乘方法，估计出飞行器质心参数，再由已估计出质心值，采用带遗忘因子递推最小二乘方法估计飞行器转动惯量，循环迭代，估计出较精确质心参数，然后传递给预摆角计算模块。

优选的，所述程序角去噪与补偿模块实现过程如下：

制导计算模块周期性向姿轨耦合控制系统发出飞行器需要跟踪姿态角信息，该信号为本体系到目标系姿态角，对制导计算模块输出的程序角信号进行离线频谱分析，获取程序角信号的有效带宽，程序角去噪与补偿模块首先对制导计算模块输出的程序角和预摆角计算程序补偿角进行叠加处理，即将轨道系到本体系程序角转换为质心系到目标系程序角，然后将欧拉角转换为四元数，将信号发送给离散化的低通滤波差分方程计算，差分输出即为程序角去噪与补偿后程序四元数，并将输出值发送给控制反馈偏差计算模块。

优选的，所述增量式PID控制模块实现过程如下：

由于控制反馈偏差计算模块计算出误差四元数和误差姿态角速度，增量式PID模块采用增量式PID控制律为Δu(k)＝u(k)-u(k-1)＝k_p(e(k)-e(k-1))+k_ie(k)+k_dω_e,u(k)＝u(k-1)+Δu(k)，式中e为四元数误差，ω_e为姿态角速度偏差，k_p、k_i、k_d分别为比例、积分、微分系数，控制输出为飞行器俯仰、偏航通道发动机摇摆角大小，将计算结果输出给等效伺服指令及限幅模块。

一种基于发动机预摆的大质心横移下的姿轨控耦合控制方法，包括质心估计步骤、预摆角计算步骤、程序角去噪与补偿步骤、控制反馈偏差计算步骤、增量式PID控制步骤和等效伺服摆角指令及限幅步骤，具体如下：

(1)质心估计步骤，建立飞行器动力学模型，并分别以质心、转动惯量为状态变量建立质心、惯量、控制力矩、姿态信息之间的状态方程，控制计算机同时发送飞行器俯仰、偏航、滚动三通道姿控发动机开关指令，则控制力矩可根据理论发动机推力及安装位置计算出，假定转动惯量已知，通过敏感器采集飞行器角速度、角加速度信息，建立质心估计模型，通过带遗忘因子递推最小二乘方法，估计出质心参数，再由已估计出质心参数，建立转动惯量估计模型，采用同样方法可以估计出转动惯量值，循环迭代，控制计算机发送三通道姿控发动机正开、负开交替指令，以便准确估计出飞行器质心参数；

(2)预摆角计算步骤，根据质心估计步骤输出质心参数，采用坐标系旋转的方式，获取本体系与质心系的旋转角度，以此计算出发动机对准质心俯仰、偏航通道发动机摆角值及程序角补偿值，并分别发送给程序角去噪与补偿步骤和等效伺服摆角指令及限幅步骤；

(3)程序角去噪与补偿步骤，通过对制导计算程序角进行FFT频谱分析，确定程序角信号带宽，设计二阶低通滤波器，并根据控制计算机计算周期转换为差分方程，将制导计算步骤输出的程序角和预摆角计算步骤输出的程序角补偿量，通过坐标旋转获取补偿后程序角，经过差分方程计算，获取滤波后程序角，然后通过姿态角及四院数转换方程，转换为四元数，传递给控制反馈偏差计算步骤；

(4)控制反馈偏差计算步骤，由导航模块输出导航四元数和程序角去噪与补偿步骤输出的程序四元数，采用四元数叉乘方程，获取四元数偏差值，由导航模块输出导航姿态角速度，获取姿态角速度偏差值，将计算出四元数偏差及姿态角速度偏差值发送给增量式PID控制步骤；

(5)增量式PID控制步骤，根据控制反馈偏差计算步骤输出的四元数偏差和姿态角速度偏差，采用增量式PID控制律计算出俯仰、偏航通道摆角控制指令，将摆角指令传递给等效伺服摆角指令及限幅步骤；

(6)等效伺服摆角指令及限幅步骤，接收增量式PID控制步骤输出的俯仰、偏航通道摆角控制指令以及预摆角计算步骤输出的发动机预摆角指令，将两个指令信号进行叠加，根据伺服安装位置，将摆角指令分配为伺服电机I、II控制指令，并根据发动机摇摆范围，对伺服电机控制指令进行限幅，并将限幅后的伺服电机控制指令发送给综合控制器。

本发明与现有技术相比的优点在于：

(1)本发明通过在线质心估计、发动机预摆、误差四元数反馈控制、增量式PID算法、系统有效带宽分析和去噪技术，解决了在轨飞行器大质心横移姿轨耦合的高精度控制，达到了很好的工程应用效果。

(2)本发明提出基于质心估计的发动机预摆和制导程序角补偿方案，在发动机点火之前，采用伺服系统预摆发动机对准质心，则点火时质心横移引起的干扰力矩便会降低或消除，并采用在线质心估计的方法获取较为精确的质心横移值，达到精确预摆的目的。同时由于质心横移的存在，使得飞行器制导程序角基准发生改变，采用在线对制导程序角进行补偿，提高制导精度。

(3)本发明采用基于误差四元数反馈的增量式PID控制方案，有效解决欧拉角奇异问题，解决飞行器大角度机动过程姿态非单值问题，较传统运载控制方法，加入增量式积分环节，有效降低系统稳态误差，降低比例、微分环节参数幅值，达到在满足稳定控制精度的基础上有效增大系统抗干扰能力的目的。

(4)本发明采用有效系统带宽频谱分析和去噪技术，避免高频干扰信号引入姿控系统，此方法较传统姿控系统控制中加校正网络降低系统带宽的方案，优点为姿态控制相位、幅值裕度均较好，可更好的跟踪制导信号。

附图说明

图1为本发明的系统组成框图；

图2为本发明中预摆计算示意图；

图3为本发明中程序角补偿计算示意图；

图4为本发明中增量式PID控制方法；

图5为本发明中基于动力学质心估计示意图；

具体实施方式

以下将结合附图和实施例对本发明作进一步详细描述：

如图1所示，本发明基于发动机预摆的大质心横移下的姿轨控耦合控制系统包括：质心估计模块、预摆角计算模块、程序角去噪与补偿模块、控制反馈偏差计算模块、增量式PID控制模块、等效伺服摆角指令及限幅模块。其工作流程为质心估计模块计估计出飞行器质心值，发送给预摆角计算模块，预摆角计算模块计算出发动机预摆角及程序角补偿值，发动给等效摆角计算及限幅模块和程序角去噪与补偿模块。程序角去噪与补偿模块计算出滤波后程序角四元数发送给控制反馈偏差计算模块，控制反馈计算模块计算出误差四元数和误差姿态角速度发送给增量式PID控制模块，增量式PID控制模块计算出俯仰、偏航通道发动机摇摆指令，发送给等效伺服摆角指令及限幅模块，等效伺服摆角指令及限幅模块计算出伺服机构I、II控制指令，发送给综合控制器。

如图5所示，质心估计模块，分别建立飞行器运动学模型和姿态动力学模型，将动力学方程中的外力/外力矩和状态信息通过测量等手段作为已知量，将质心、惯量等特性参数作为未知量，然后整理成最小二乘法的标准形式进行未知参数的估计，采用带遗忘因子递推二乘法进行估计。首先建立刚性飞行器的姿态动力学方程为

式中：

I为飞行器的转动惯量，ω为飞行器相对于惯性空间的角速度，T_c为提供的控制力矩，T_d为外部环境干扰力矩，q^T＝[q₁,q₂,q₃,q₄]，并满足极线约束q^Tq＝1。

选择转动惯量参数作为状态变量，即X＝[I_xx I_yy I_zz I_xy I_xz I_yz]^T。令b＝T_c+T_d作为输出变量，则姿态动力学方程写成矩阵的形式为b＝AX。

式中：

对于转动惯量辨识观测方程，均选择b作为观测量，选择X作为状态变量，则转动惯量辨识观测方程可以统一描述为Z＝H(X)。根据转动惯量辨识观测方程，采用扩展卡尔曼滤波方法设计辨识算法。为了采用带遗忘因子递推最小二乘方法，将观测方程转换成离散状态方程为

X_k+1＝X_k

Z_k＝H_kX_k

转动惯量估计，具体的算法流程为

第一步，设定初始值：X₀和P₀

第二步，建立递推方程

式中P_k|k-1,P_k,K_k为误差矩阵，H为状态矩阵，γ为遗忘因子。则方程构成了飞行器的转动惯量辨识方程。在转动惯量估计基础上对新的质心位置进行估计，依靠推力器和陀螺进行质心位置辨识。

(1)飞行器的x轴推力器工作，由于飞行器质心不过形心，因此会产生y轴和z轴的姿态干扰力矩。推力器的大小认为是已知，则干扰力矩的大小直接与推力器距离质心的距离相关。在利用陀螺测量得到箭体的姿态角速度，根据姿态动力学方程，将推力器与质心之间的距离作为未知量，整理成最小二乘法的标准形式，最终可得到上面级质心在飞行器本体系上的y轴和z轴坐标，而x轴上的坐标无法确定，因此联合体质心是在平行于x轴的直线上。

(2)同理，使用y轴和z轴上的推力器分别工作，可分别估计得到上面级质心位置所在的直线。

(3)理论上三条直线应该相交于同一点，该点即为飞行器的质心。但受到动力学模型误差、测量误差、推力误差等因素的影响，最终三条直线可能不会相交于同一点，则依靠质心在同一方向上的不同估计值进行融合处理，得到最终的飞行器质心三轴位置坐标。

如图2所示，预摆角计算模块，飞行器分离卫星之后，质心存在横向偏移量。为了克服质心横移下轨控对姿态控制的影响，提出发动机预摆方案，让发动机点火之前推力方向对准质心。发动机预摆角大小为

式中(x_c,y_c,z_c)为飞行器质心在布局坐标系下坐标，为质心估计模块输出值，d为摇摆点在布局坐标系x轴坐标。

如图3所示，预摆角计算模块，姿态补偿角在数值上等于摆角补偿角δ₀，符号相反，旋转顺序相反。采用发动机预摆方案后，发动机摆角补偿角依靠理论计算得出

如图2所示，程序角去噪与补偿模块，针对上面级的任务特点，上面级制导方案采用显式制导方法。根据终端目标参数和飞行器现在的运动参数对控制量的显式表达式实时计算控制姿态角，一般制导周期大于姿态控制周期，加入最优程序角后会使发动机摆角产生高频的振荡，对于制导程序角频谱分析，可知程序角带宽范围，因此，稳定系统对最优程序角进行滤波，通过滤波网络目的是平滑程序角输入信号，减小对稳定系统的影响。

滤波网络设计函数为

式中ξ₁,ξ₂为阻尼，ω₁,ω₂为频率。离散化处理如下

F₁ ^α(nT_filter)＝α_cx

式中：为α通道滤波方程系数，表示俯仰、偏航通道；α_cx，为各通道最终程序角处理后的当前程序角；为当前拍程序角滤波输出值。滤波后程序角转化为四元数为

式中：为滤波后俯仰、偏航程序角，q_cx0为程序四元数，程序角补偿后四元数为

式中：q_yb为程序角补偿四元数，q_cx为补偿后程序角四元数。

如图1所示，控制量偏差计算模块，计算公式如下

q_e＝q_e/|q_e|·sign(q_e(0))

式中：

Q_gb为姿态四元数，由导航模块计算得到；

q_cx为当前程序四元数，由程序角去噪与补偿模块计算得到；

q_e为四元数控制偏差；

ω_e为角速度控制偏差；

ω_cx为程序角速度，由程序角去噪与补偿模块计算得到；

为当前平均角速度值，由导航模块计算得到；

如图4所示，增量式PID控制模块，计算公式如下

δ_ψ＝u_ψ(k)

式中：

q_e为四元数控制偏差；

ω_e为角速度控制偏差；

k_pψ、k_dψ：PID控制参数

如图1所示，等效伺服摆角指令及限幅模块将增量式输出指令与预摆指令进行叠加，如下公式

δ_ψ＝δ_ψ+δ_{ψ_yb}

式中δ_{ψ_yb}为预摆角，由预摆角计算模块计算得到。

根据发动机摆动范围限制级伺服安装位置进行伺服指令及限幅计算，如下式所示

式中：

δ_ψout：输出的伺服摆角；

δ_max：伺服摆角限幅值，常值装订；

ΔT_zk：姿控周期，常值装订；

δ_{ψ_yb}：预摆角。

将经过分配限幅的伺服控制指令发送给综合控制器，控制伺服机构摇摆。

Claims (5)

1.一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统，其特征在于，包括：质心估计模块、预摆角计算模块、程序角去噪与补偿模块、控制反馈偏差计算模块、增量式PID控制模块、等效伺服摆角指令及限幅模块；

质心估计模块，根据控制计算机发送姿控发动控制指令，采集敏感器信息，通过估计方法，实时估计飞行器质心参数，然后传递给预摆角计算模块；

预摆角计算模块，根据质心估计模块实时估计飞行器质心参数计算出发动机预摆角及程序角补偿值，将程序角补偿值传递给程序角去噪与补偿模块对程序角进行补偿，将发动机预摆角发送给等效伺服摆角指令及限幅模块，对伺服摆角进行预摆处理；

程序角去噪与补偿模块，对制导计算模块输出的程序角指令进行滤波处理，然后根据预摆角计算模块输出程序角补偿值，将制导输出程序角由本体系旋转到质心系，将补偿后程序角输出给控制反馈偏差计算模块；

控制反馈偏差计算模块，输入为导航模块计算飞行器当前相对于轨道系姿态信息和由程序角去噪与补偿模块输出的程序角，然后进行四元数及姿态角速度偏差求解，并将偏差值输出给增量式PID控制模块；

增量式PID控制模块，根据控制反馈偏差计算模块输出的四元数偏差和姿态角速度偏差，采用增量式PID控制律计算出俯仰、偏航通道摆角控制指令，将摆角控制指令传递给等效伺服摆角指令及限幅模块；

等效伺服摆角指令计算及限幅模块，接收增量式PID控制模块输出的俯仰、偏航通道摆角控制指令以及预摆角计算模块输出的发动机预摆角指令，将两个指令信号进行叠加，根据伺服安装位置，将摆角指令分配为伺服电机I、II控制指令，并根据发动机摇摆范围，对伺服电机控制指令进行限幅，并将限幅后的伺服电机控制指令发送给综合控制器。

2.根据权利要求1所述基于发动机预摆的大质心横移下的姿轨控耦合控制系统，其特征在于：质心估计模块实现过程如下：

控制计算机分别对飞行器三通道正负方向发送定时姿控发动机开关指令，假定姿控发动机推力及力臂已知，即控制力矩系数已知，给定转动动量初值，由敏感器测量姿态角速度及姿态角加速度信息，从这些数据建立仿射系统本体属性的数学模型，采用带遗忘因子递推最小二乘方法，估计出飞行器质心参数，再由已估计出质心值，采用带遗忘因子递推最小二乘方法估计飞行器转动惯量，循环迭代，估计出较精确质心参数，然后传递给预摆角计算模块。

3.根据权利要求1所述基于发动机预摆的大质心横移下的姿轨控耦合控制系统，其特征在于：所述程序角去噪与补偿模块实现过程如下：

制导计算模块周期性向姿轨耦合控制系统发出飞行器需要跟踪姿态角信息，该信号为本体系到目标系姿态角，对制导计算模块输出的程序角信号进行离线频谱分析，获取程序角信号的有效带宽，程序角去噪与补偿模块首先对制导计算模块输出的程序角和预摆角计算程序补偿角进行叠加处理，即将轨道系到本体系程序角转换为质心系到目标系程序角，然后将欧拉角转换为四元数，将信号发送给离散化的低通滤波差分方程计算，差分输出即为程序角去噪与补偿后程序四元数，并将输出值发送给控制反馈偏差计算模块。

4.根据权利要求1所述基于发动机预摆的大质心横移下的姿轨控耦合控制系统，其特征在于：所述增量式PID控制模块实现过程如下：

由于控制反馈偏差计算模块计算出误差四元数和误差姿态角速度，增量式PID模块采用增量式PID控制律为Δu(k)＝u(k)-u(k-1)＝k_p(e(k)-e(k-1))+k_ie(k)+k_dω_e,u(k)＝u(k-1)+Δu(k)，式中e为四元数误差，ω_e为姿态角速度偏差，k_p、k_i、k_d分别为比例、积分、微分系数，控制输出为飞行器俯仰、偏航通道发动机摇摆角大小，将计算结果输出给等效伺服指令及限幅模块。

5.一种基于发动机预摆的大质心横移下的姿轨控耦合控制方法，其特征在于包括质心估计步骤、预摆角计算步骤、程序角去噪与补偿步骤、控制反馈偏差计算步骤、增量式PID控制步骤和等效伺服摆角指令及限幅步骤，具体如下：

(1)质心估计步骤，建立飞行器动力学模型，并分别以质心、转动惯量为状态变量建立质心、惯量、控制力矩、姿态信息之间的状态方程，控制计算机同时发送飞行器俯仰、偏航、滚动三通道姿控发动机开关指令，则控制力矩可根据理论发动机推力及安装位置计算出，假定转动惯量已知，通过敏感器采集飞行器角速度、角加速度信息，建立质心估计模型，通过带遗忘因子递推最小二乘方法，估计出质心参数，再由已估计出质心参数，建立转动惯量估计模型，采用同样方法可以估计出转动惯量值，循环迭代，控制计算机发送三通道姿控发动机正开、负开交替指令，以便准确估计出飞行器质心参数；

(2)预摆角计算步骤，根据质心估计步骤输出质心参数，采用坐标系旋转的方式，获取本体系与质心系的旋转角度，以此计算出发动机对准质心俯仰、偏航通道发动机摆角值及程序角补偿值，并分别发送给程序角去噪与补偿步骤和等效伺服摆角指令及限幅步骤；

(3)程序角去噪与补偿步骤，通过对制导计算程序角进行FFT频谱分析，确定程序角信号带宽，设计二阶低通滤波器，并根据控制计算机计算周期转换为差分方程，将制导计算步骤输出的程序角和预摆角计算步骤输出的程序角补偿量，通过坐标旋转获取补偿后程序角，经过差分方程计算，获取滤波后程序角，然后通过姿态角及四院数转换方程，转换为四元数，传递给控制反馈偏差计算步骤；

(4)控制反馈偏差计算步骤，由导航模块输出导航四元数和程序角去噪与补偿步骤输出的程序四元数，采用四元数叉乘方程，获取四元数偏差值，由导航模块输出导航姿态角速度，获取姿态角速度偏差值，将计算出四元数偏差及姿态角速度偏差值发送给增量式PID控制步骤；

(5)增量式PID控制步骤，根据控制反馈偏差计算步骤输出的四元数偏差和姿态角速度偏差，采用增量式PID控制律计算出俯仰、偏航通道摆角控制指令，将摆角指令传递给等效伺服摆角指令及限幅步骤；

(6)等效伺服摆角指令及限幅步骤，接收增量式PID控制步骤输出的俯仰、偏航通道摆角控制指令以及预摆角计算步骤输出的发动机预摆角指令，将两个指令信号进行叠加，根据伺服安装位置，将摆角指令分配为伺服电机I、II控制指令，并根据发动机摇摆范围，对伺服电机控制指令进行限幅，并将限幅后的伺服电机控制指令发送给综合控制器。