推力变向无人机的姿态控制系统及控制方法

摘要

本发明公开了一种推力变向无人机的姿态控制系统及控制方法。本发明首次建立了推力变向无人机的非线性全量数学模型和线性化纵横向数学模型，并利用融合估计和最优控制的方法，提出了本发明的基于姿态回路信息融合的推力变向无人机的姿态控制系统及控制方法。对于推力变向无人机的设计开发具有极为重要的现实意义。本发明在控制性能方面优于传统单变量控制方法，且计算简单，完全满足实际飞行控制系统的实时性要求。

主权项

1.一种推力变向无人机的姿态控制系统，所述推力变向无人机采用单发动机，发动机安装位置处于飞机纵向对称面上，所述姿态控制系统包括根据给定姿态指令信号解算控制律，得到姿态控制信号并将姿态控制信号输出给执行机构的姿态控制器，其特征在于，所述姿态控制器包括纵向姿态信息融合控制器和横向姿态信息融合控制器；所述纵向姿态信息融合控制器的输入为期望的飞行速度增量ΔV^*、期望的俯仰角增量Δθ^*、期望的迎角增量Δα^*、期望的俯仰角速度增量Δq^*、传感器反馈的俯仰角速度增量Δq、传感器反馈的俯仰角增量Δθ、传感器反馈的迎角增量Δα、传感器反馈的飞行速度增量ΔV、期望的升降舵偏角增量Δδ_e^*、油门开度增量Δδ_T^*、发动机推力纵向偏角增量Δα_T^*；输出为升降舵偏角增量Δδ_e、油门开度增量Δδ_T、发动机推力纵向偏角增量Δα_T，其信息融合最优控制律如下，${\hat{U}}_{\mathrm{lon}}\left(k\right)={[R_{\mathrm{lon}}\left(k\right)+{B_{\mathrm{lon}}}^T\left(k\right){P_{\mathrm{lon}}}^{-1}(k+1)B_{\mathrm{lon}}\left(k\right)]}^{-1}\{{B_{\mathrm{lon}}}^T\left(k\right){P_{\mathrm{lon}}}^{-1}(k+1)$$[{\hat{X}}_{\mathrm{lon}}(k+1)-A_{\mathrm{lon}}\left(k\right)X_{\mathrm{lon}}\left(k\right)]+R_{\mathrm{lon}}\left(k\right){U_{\mathrm{lon}}}^{*}\left(k\right)\}$式中，为虚拟纵向状态向量X_lon的融合估计，p_lon^-1为融合估计的融合信息量，且融合估计及其信息量P_lon^-1由下面公式逆向递推计算获得，$\left\{\begin{array}{c}{\hat{X}}_{\mathrm{lon}}\left(j\right)=P_{\mathrm{lon}}\left(j\right)\left\{{A_{\mathrm{lon}}}^T\left(j\right){[P_{\mathrm{lon}}(j+1)+B_{\mathrm{lon}}\left(j\right){R_{\mathrm{lon}}}^{-1}\left(j\right){B_{\mathrm{lon}}}^T\left(j\right)]}^{-1}\right[{\hat{X}}_{\mathrm{lon}}(j+1)-B_{\mathrm{lon}}\left(j\right){U_{\mathrm{lon}}}^{*}\left(j\right)]-Q_{\mathrm{lon}}\left(j\right){X_{\mathrm{lon}}}^{*}\left(j\right)\}\\ {P_{\mathrm{lon}}}^{-1}\left(j\right)=Q_{\mathrm{lon}}\left(j\right)+{A_{\mathrm{lon}}}^T\left(j\right){[P_{\mathrm{lon}}(j+1)+B_{\mathrm{lon}}\left(j\right){R_{\mathrm{lon}}}^{-1}\left(j\right){B_{\mathrm{lon}}}^T\left(j\right)]}^{-1}{A}_{\mathrm{lon}}\left(j\right),{P_{\mathrm{lon}}}^{-1}\left(N\right)=Q_{\mathrm{lon}}\left(N\right)\\ j=k+1,k+2,...,N-1\end{array}\right.,$式中，X_lon＝[ΔV，Δα，Δθ，Δq]^T，U_lon＝[Δδ_T，Δδ_e，Δα_T]^T，A_lon(k)，B_lon(k)为连续时间纵向线性化运动方程经过离散化后的系统矩阵，R_lon(k)，Q_lon(k)为最优性能指标权重矩阵，X_lon^*＝[ΔV^*，Δα^*，Δθ^*，Δq^*]^T为期望的纵向状态向量，U_lon^*＝[Δδ_T^*，Δδ_e^*，Δα_T^*]^T为期望的纵向控制向量；所述横向姿态信息融合控制器的输入为期望的侧滑角增量Δβ^*、期望的滚转角增量Δφ^*、期望的滚转角速度增量Δp^*、期望的偏航角速度增量Δr^*、传感器反馈的滚转角速度增量Δp、传感器反馈的滚转角增量Δφ、传感器反馈的偏航角速度增量Δr、传感器反馈的侧滑角增量Δβ、期望的副翼偏角增量Δδ_α^*、期望的方向舵偏角增量Δδ_r^*、期望的发动机推力横向偏角增量Δβ_T^*，输出为副翼偏角增量Δδ_α、方向舵偏角增量Δδ_r、发动机推力横向偏角增量Δβ_T，其信息融合最优控制律如下，${\hat{U}}_{\mathrm{lat}}\left(k\right)={[R_{\mathrm{lat}}\left(k\right)+{B_{\mathrm{lat}}}^T\left(k\right){P_{\mathrm{lat}}}^{-1}(k+1)B_{\mathrm{lat}}\left(k\right)]}^{-1}\{{B_{\mathrm{lat}}}^T\left(k\right){P_{\mathrm{lat}}}^{-1}(k+1)$$[{\hat{X}}_{\mathrm{lat}}(k+1)-A_{\mathrm{lat}}\left(k\right)X_{\mathrm{lat}}\left(k\right)]+R_{\mathrm{lat}}\left(k\right){U_{\mathrm{lat}}}^{*}\left(k\right)\},$式中，为虚拟横向状态向量X_lat的融合估计，P_lat^-1为融合估计的融合信息量，且融合估计及其信息量P_lat^-1由下面公式逆向递推计算获得，$\left\{\begin{array}{c}{\hat{X}}_{\mathrm{lat}}\left(j\right)=P_{\mathrm{lat}}\left(j\right)\left\{{A_{\mathrm{lat}}}^T\left(j\right){[P_{\mathrm{lat}}(j+1)+B_{\mathrm{lat}}\left(j\right){R_{\mathrm{lat}}}^{-1}\left(j\right){B_{\mathrm{lat}}}^T\left(j\right)]}^{-1}\right[{\hat{X}}_{\mathrm{lat}}(j+1)-B_{\mathrm{lat}}\left(j\right){U_{\mathrm{lat}}}^{*}\left(j\right)]+Q_{\mathrm{lat}}\left(j\right){X_{\mathrm{lat}}}^{*}\left(j\right)\}\\ {P_{\mathrm{lat}}}^{-1}\left(j\right)=Q_{\mathrm{lat}}\left(j\right)+{A_{\mathrm{lat}}}^T\left(j\right){[P_{\mathrm{lat}}(j+1)+B_{\mathrm{lat}}\left(j\right){R_{\mathrm{lat}}}^{-1}\left(j\right){B_{\mathrm{lat}}}^T\left(j\right)]}^{-1}{A}_{\mathrm{lat}}\left(j\right),{P_{\mathrm{lat}}}^{-1}\left(N\right)=Q_{\mathrm{lat}}\left(N\right)\\ j=k+1,k+2,...,N-1\end{array}\right.,$式中，X_lat＝[Δβ，Δφ，Δp，Δr]^T，U_lat＝[Δδ_a，Δδ_r，Δβ_T]^T，A_lat(k)，B_lat(k)为连续时间横侧向线性化运动方程经过离散化后的系统矩阵，R_lat(k)，Q_lat(k)为最优性能指标权重矩阵，X_lat^*[Δβ^*，Δφ^*，Δp^*，Δr^*]^T为期望的横向状态向量，U_lat^*＝[Δδ_a^*，Δδ_r^*，Δβ_T^*]^T为期望的横向控制向量，其它参数为中间矩阵变量；所述纵向线性化运动方程如下式所示，${\stackrel{·}{X}}_{\mathrm{lon}}=A_{\mathrm{lon}}{X}_{\mathrm{lon}}+B_{\mathrm{lon}}{U}_{\mathrm{lon}},$式中，X_lon＝[ΔV，Δα，Δθ，Δq]^TU_lon＝[Δδ_T，Δδ_e，Δα_T]^T$A_{\mathrm{lon}}=\left[\begin{array}{cccc}\frac{T_V{\cos \alpha }^{*}-D_V}{m}& \frac{T^{*}{\sin \alpha }^{*}+D_{\alpha }-{\mathrm{mg}\cos \mu }^{*}}{m}& -g\cos {\mu }^{*}& 0\\ \frac{T_V{\sin \alpha }^{*}+L_V}{L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*}}& \frac{T{\cos \alpha }^{*}+L_{\alpha }-{\mathrm{mg}\sin \mu }^{*}}{L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*}}& \frac{{\mathrm{mg}\sin \mu }^{*}}{L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*}}& \frac{L_q-{\mathrm{mV}}^{*}}{L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*}}\\ 0& 0& 0& 1\\ \frac{M_V}{I_y}-M_{\stackrel{·}{\alpha }}\frac{T_V{\sin \alpha }^{*}+L_V}{I_y(L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*})}& \frac{M_{\alpha }}{I_y}-M_{\stackrel{·}{\alpha }}\frac{T^{*}{\cos \alpha }^{*}+L_{\alpha }-{\mathrm{mg}\sin \mu }^{*}}{I_y(L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*})}& \frac{M_{\stackrel{·}{\alpha }}{\mathrm{mg}\sin \mu }^{*}}{I_y(L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*})}& \frac{M_q}{I_y}-M_{\stackrel{·}{\alpha }}\frac{L_q-{\mathrm{mV}}^{*}}{I_y(L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*})}\end{array}\right]$$B_{\mathrm{lon}}=\left[\begin{array}{ccc}\frac{T_{\mathrm{\delta T}}{\cos \alpha }^{*}}{m}& \frac{{-D}_{\mathrm{\delta e}}}{m}& \frac{T^{*}{\sin \alpha }^{*}}{m}\\ \frac{{-T}_{\mathrm{\delta T}}{\sin \alpha }^{*}}{L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*}}& \frac{{-L}_{\mathrm{\delta e}}}{L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*}}& \frac{T^{*}{\cos \alpha }^{*}}{L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*}}\\ 0& 0& 0\\ -\frac{M_{\stackrel{·}{\alpha }}{T}_{\mathrm{\delta T}}\sin {\alpha }^{*}}{I_y(L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*})}& \frac{M_{\mathrm{\delta e}}}{I_y}-\frac{M_{\stackrel{·}{\alpha }}{L}_{\mathrm{\delta e}}}{I_y(L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*})}& \frac{M_{\mathrm{\alpha T}}}{I_y}+\frac{M_{\stackrel{·}{\alpha }}{T}^{*}{\cos \alpha }^{*}}{I_y(L_{\stackrel{·}{\alpha }}+{\mathrm{mV}}^{*})}\end{array}\right];$所述横侧向线性化运动方程如下式所示，${\stackrel{·}{X}}_{\mathrm{lat}}=A_{\mathrm{lat}}{X}_{\mathrm{lat}}+B_{\mathrm{lat}}{U}_{\mathrm{lat}},$式中，X_lat＝[Δβ，Δφ，Δp，Δr]^T，U_lat＝[Δδ_a，Δδ_r，Δβ_T]^T，$A_{\mathrm{lat}}=\left[\begin{array}{cccc}\frac{Y_{\beta }}{{\mathrm{mV}}^{*}}& \frac{g\cos {\theta }^{*}}{V^{*}}& {\sin \alpha }^{*}+\frac{Y_p}{{\mathrm{mV}}^{*}}& \frac{Y_r}{{\mathrm{mV}}^{*}}-{\cos \alpha }^{*}\\ 0& 0& 1& {\tan \theta }^{*}\\ \frac{I_z{\bar{L}}_{\beta }+I_{\mathrm{zx}}{N}_{\beta }}{I_x{I}_z-{I_{\mathrm{zx}}}^2}& 0& \frac{I_z{\bar{L}}_p+I_{\mathrm{zx}}{N}_p}{I_x{I}_z-{I_{\mathrm{zx}}}^2}& \frac{I_z{\bar{L}}_r+I_{\mathrm{zx}}{N}_r}{I_x{I}_z-{I_{\mathrm{zx}}}^2}\\ \frac{I_{\mathrm{zx}}{\bar{L}}_{\beta }+I_x{N}_{\beta }}{I_x{I}_z-{I_{\mathrm{zx}}}^2}& 0& \frac{I_{\mathrm{zx}}{\bar{L}}_p+I_x{N}_p}{I_x{I}_z-{I_{\mathrm{zx}}}^2}& \frac{I_{\mathrm{zx}}{\bar{L}}_r+I_x{N}_r}{I_x{I}_z-{I_{\mathrm{zx}}}^2}\end{array}\right],$$B_{\mathrm{lat}}=\left[\begin{array}{ccc}\frac{Y_{\mathrm{\delta a}}}{{\mathrm{mV}}^{*}}& \frac{Y_{\mathrm{\delta r}}}{{\mathrm{mV}}^{*}}& \frac{T^{*}}{{\mathrm{mV}}^{*}}\\ 0& 0& 0\\ \frac{I_z{\bar{L}}_{\mathrm{\delta a}}+I_{\mathrm{zx}}{N}_{\mathrm{\delta a}}}{I_x{I}_z-{I_{\mathrm{zx}}}^2}& \frac{I_z{\bar{L}}_{\mathrm{\delta r}}+I_{\mathrm{zx}}{N}_{\mathrm{\delta r}}}{I_x{I}_z-{I_{\mathrm{zx}}}^2}& \frac{I_z{\bar{L}}_{\mathrm{\beta T}}+I_{\mathrm{zx}}{N}_{\mathrm{\beta T}}}{I_z{I}_x-{I_{\mathrm{zx}}}^2}\\ \frac{I_{\mathrm{zx}}{\bar{L}}_{\mathrm{\delta a}}+I_x{N}_{\mathrm{\delta a}}}{I_x{I}_z-{I_{\mathrm{zx}}}^2}& \frac{I_{\mathrm{zx}}{\bar{L}}_{\mathrm{\delta r}}+I_x{N}_{\mathrm{\delta r}}}{I_x{I}_z-{I_{\mathrm{zx}}}^2}& \frac{I_{\mathrm{zx}}{\bar{L}}_{\mathrm{\beta T}}+I_x{N}_{\mathrm{\beta T}}}{I_z{I}_x-{I_{\mathrm{zx}}}^2}\end{array}\right]$其中所涉及的字母含义如下：V为飞行速度，α为迎角，θ为俯仰角，q为俯仰角速度，δ_T为发动机油门开度，δ_e为升降舵偏角，α_T为推力纵向偏角，T_V为发动机推力T对飞行速度V的偏导数，T_δT为发动机推力T对发动机油门开度δ_T的偏导数，L_V为空气升力L对飞行速度V的偏导数，L_α为空气升力L对迎角α的偏导数，L_q为空气升力L对俯仰角速度q的偏导数，为空气升力L对迎角变化率的偏导数，L_δe为空气升力L对升降舵偏角δ_e的偏导数，D_V为空气阻力D对飞行速度V的偏导数，D_α为空气阻力D对迎角α的偏导数，D_δe为空气阻力D对升降舵偏角δ_e的偏导数，M_V为俯仰力矩M对飞行速度V的偏导数，为俯仰力矩M对迎角变化率的偏导数，M_α为俯仰力矩M对迎角α的偏导数，M_q为俯仰力矩M对俯仰角速度q的偏导数，M_δe为俯仰力矩M对升降舵偏角δ_e的偏导数，M_αT为俯仰力矩M对推力纵向偏角α_T的偏导数，I_y为飞机刚体绕机体坐标系y轴的转动惯量，μ为航迹倾斜角，m为飞机质量，g为重力加速度，β为侧滑角，φ为滚转角，p为滚转角速度，r为偏航角速度，δ_a为副翼偏角，δ_r为方向舵偏角，β_T为推力横向偏角，Y_β为侧力Y对侧滑角β的偏导数，Y_p为侧力Y对滚转角速度p的偏导数，Y_r为侧力Y对偏航角速度r的偏导数，Y_δa为侧力Y对副翼偏角δ_a的偏导数，Y_δr为侧力Y对方向舵偏角δ_r的偏导数，I_z为飞机刚体绕机体坐标系z轴的转动惯量，I_x为飞机刚体绕机体坐标系x轴的转动惯量，I_zx为飞机刚体对坐标系z轴、x轴的惯性积，为滚转力矩对侧滑角β的偏导数，为滚转力矩对滚转角速度p的偏导数，为滚转力矩对偏航角速度r的偏导数，为滚转力矩对副翼偏角δ_a的偏导数，为滚转力矩对方向舵偏角δ_r的偏导数，为滚转力矩对推力横向偏角β_T的偏导数，N_β为偏航力矩N对侧滑角β的偏导数，N_p为偏航力矩N对滚转角速度p的偏导数，N_r为偏航力矩N对偏航角速度r的偏导数，N_δa为偏航力矩N对副翼偏角δ_a的偏导数，N_δr为偏航力矩N对方向舵偏角δ_r的偏导数，N_βT为偏航力矩N对推力横向偏角β_T的偏导数，变量前面加Δ表示该变量的增量，变量右上角加*表示该变量的平衡态基准值。