CN102520726B - 大攻角飞行状态下的大气攻角及侧滑角估计方法 - Google Patents

Abstract

本发明公开了一种大攻角飞行状态下的大气攻角及侧滑角估计方法，在不改变飞行器结构外形、不增添额外测量装置和硬件设备的前提下，充分利用现有机载捷联惯性导航***、GPS***、飞行控制***以及燃油测量***输出的参数，解算飞行器的角加速度、转动惯量、惯性积、力矩方程组的系数、总质量，基于飞行动力学模型，通过构建扩展卡尔曼滤波器，融飞行动力学模型求解和状态估计的过程为一体，实现当前时刻攻角、侧滑角和真空速的精确估计，进而解算出当前时刻的风速及风速变化率，并作为反馈信息输入到扩展卡尔曼滤波器中，完成下一时刻攻角及侧滑角的实时精确估计，通过递推求解的方式实现攻角和侧滑角的实时精确估计，提高了大气数据***的测量范围和测量精度以及对复杂飞行环境的适应性。

Description

大攻角飞行状态下的大气攻角及侧滑角估计方法

技术领域

本发明涉及一种大攻角飞行状态下的大气攻角及侧滑角估计方法，属于大气数据、飞行器控制、惯性导航及GPS***等技术领域。

背景技术

攻角和侧滑角是飞行器飞行状态和飞行力学的两个重要参数，其精度和可靠性直接关系到飞行器控制、导航、发动机及火控等***的正常工作及性能发挥。现代高性能战机的主要战术性能要求包括超机动能力、超音速巡航、隐身性、高信息优势等。高机动飞行意味着飞行器具备大攻角飞行能力，大攻角飞行引起机体表面气流发生分离，飞行器处于非线性漩涡气流中，目前的攻角和侧滑角主要是通过安装在飞行器上的风标传感器、压差式传感器和零压差式传感器等来测量，这些类型的攻角和侧滑角传感器对气流扰动比较敏感，导致攻角和侧滑角传感器性能严重下降，难以满足大攻角飞行范围内攻角和侧滑角的精确测量，严重制约了高性能飞行器的性能发挥。另外，突出的传感器测量装置势必会降低飞行器的隐身性，因此解决高性能飞行器攻角和侧滑角的测量问题显得尤为重要。

目前国内外都已开展了大攻角下攻角和侧滑角的测量方法研究。1991年美国NASA报告刊登的一篇“Development of a Pneumatic High-Angle-of-Attack Flush Airdata Sensing(HI-FADS)System”论文，文中提出了一种依靠嵌入在飞行器前端不同位置上的压力传感器阵列来测量飞行器表面的压力分布，并由压力分布推算大气参数的嵌入式大气数据***，该方法能够实现大攻角飞行环境下的攻角和侧滑角的测量；2009年美国专利US7,564,539B2：“Optical Air Data Systems Methods”，提出了一种采用激光与大气分子和气溶胶分别发生瑞利散射和米氏散射效应，根据后向散射信号的强度和多普勒频移解算大气参数的光学式大气数据***，该方法在测量大气参数时不受飞行状态影响，满足大攻角飞行状态下攻角和侧滑角的测量；以上两种采用研究新型的嵌入式大气数据***和光学式大气数据***的方法技术难度大且代价较高；2006年AIAA刊登了会议“AIAA Guidance，Navigation，and Control Conference and Exhibit”上的一篇论文“Flight Testing of the X-45AJ-UCAS Computational Alpha-Beta System”，提供了一种利用惯性导航***、飞行控制***以及大气数据***进行攻角和侧滑角估计方法，以此避开了大攻角特性对飞行攻角、侧滑角测量的影响，但是该方法由于没有考虑飞行器周围实际风速的影响，估计的攻角和侧滑角存在***误差。

发明内容

本发明目的是针对现有大攻角大气参数测量技术存在的不足，充分利用机载捷联惯性导航***、GPS、飞行控制***以及燃油测量***的输出参数，基于飞行动力学模型，提出一种大攻角飞行状态下的大气攻角及侧滑角估计方法，该方法具有不受飞行环境和飞行状态制约，经济有效且易于工程实现的特点。

为实现上述目的，本发明采用如下技术方案：

一种大攻角飞行状态下的大气攻角及侧滑角估计方法，其特征在于：利用飞行器机载设备输出的参数，包括：

(1)捷联惯性导航***输出的姿态角、角速度以及线加速度；

(2)GPS***输出的地速；

(3)飞行控制***输出的空气动力、外合力矩、发动机推力；

(4)燃油测量***输出的剩余燃油质量；

由角速度通过微分方法求解出角加速度；

由角速度、角加速度和外合力矩，通过最小二乘估计方法求解出飞行器的转动惯量和惯性积，进而得到力矩方程组的系数；

由剩余燃油质量和已知的飞行器机体质量、机载设备质量、乘员质量以及武器质量相加，得到飞行器总质量；

根据飞行动力学模型，构建扩展卡尔曼滤波器，利用扩展卡尔曼滤波算法，实现大攻角飞行状态下当前时刻攻角、侧滑角和真空速的精确估计；

由当前时刻的姿态角、地速以及估计出的当前时刻的攻角、侧滑角、真空速，对当前时刻的风速进行解算，并结合上一时刻的风速，对当前时刻的风速变化率进行解算；

将得到的当前时刻的风速和风速变化率反馈给扩展卡尔曼滤波模算法模块，用于完成下一时刻的攻角及侧滑角的实时精确估计，通过递推求解的方式实现攻角和侧滑角的实时精确估计。

包括以下步骤：

(1)以周期ΔT读取捷联惯性导航***中的三个角速度、三个姿态角和三个线加速度信息，三个角速度信息分别为机体坐标系下x轴、y轴和z轴方向的横滚角速度p、俯仰角速度q、偏航角速度r；三个姿态角信息分别为俯仰角θ、横滚角φ、偏航角ψ；三个线加速度信息分别为机体坐标系下x轴方向的线加速度a_x、y轴方向的线加速度a_y、z轴方向的线加速度a_z，其中机体坐标系的x轴、y轴和z轴的指向分别为向前、向右、向下；

(2)以周期ΔT读取GPS***输出的地理坐标系下的地速，地理坐标系方向定义为北、东、地，此三个方向的地速分别为u_g、v_g、w_g；

(3)以周期ΔT读取飞行控制***输出的飞行器所受空气动力、外合力矩、发动机推力，其中飞行器所受空气动力在气流坐标系下x_a轴负方向、y_a轴方向和z_a轴负方向的分量分别为阻力D、侧力Y、升力L，发动机推力在机体坐标系下x轴、y轴和z轴方向的分量分别为T_x、T_y、T_z，外合力矩在机体坐标系下x轴、y轴和z轴方向的分量分别为滚转力矩

俯仰力矩M、偏航力矩N；

(4)以周期ΔT读取飞行器燃油测量***输出的剩余燃油质量m_f；

(5)根据步骤(1)获取的机体坐标系下的横滚角速度p、俯仰角速度q、偏航角速度r，其中t时刻三个角速度信息分别为横滚角速度p(t)、俯仰角速度q(t)、偏航角速度r(t)，t+ΔT时刻三个角速度信息分别为横滚角速度p(t+ΔT)、俯仰角速度q(t+ΔT)、偏航角速度r(t+ΔT)，t时刻的机体坐标系下x轴、y轴和z轴方向的三个角加速度信息，横滚角加速度

俯仰角加速度偏航角加速度

按微分方法计算，即：

$\left\{\begin{array}{c}\stackrel{\·}{p}\left(t\right)=\frac{p(t+\mathrm{\ΔT})-p\left(t\right)}{\mathrm{\ΔT}}\\ \stackrel{\·}{q}\left(t\right)=\frac{q(t+\mathrm{\ΔT})-q\left(t\right)}{\mathrm{\ΔT}}\\ \stackrel{\·}{r}\left(t\right)=\frac{r(t+\mathrm{\ΔT})-r\left(t\right)}{\mathrm{\ΔT}}\end{array}\right.;$

(6)将已知的飞行器角运动方程组 $\left\{\begin{array}{c}\stackrel{\·}{p}{I}_x-\stackrel{\·}{r}{I}_{\mathrm{xz}}+\mathrm{qr}(I_z-I_y)-\mathrm{pq}{I}_{\mathrm{xz}}=\stackrel{\‾}{L}\\ \stackrel{\·}{q}{I}_y+\mathrm{pr}(I_x-I_z)+(p^2-r^2)I_{\mathrm{xz}}=M\\ \stackrel{\·}{r}{I}_z-\stackrel{\·}{p}{I}_{\mathrm{xz}}+\mathrm{pq}(I_y-I_x)+\mathrm{qr}{I}_{\mathrm{xz}}=N\end{array}\right.$ 的三个方程式相加，得到方程EJ＝U，其中 $E=[\stackrel{\·}{p}+\mathrm{pr}-\mathrm{pq},\stackrel{\·}{q}-\mathrm{qr}+\mathrm{pq},\stackrel{\·}{r}+\mathrm{qr}-\mathrm{pr},p^2-r^2-\stackrel{\·}{p}-\stackrel{\·}{r}-\mathrm{pq}+\mathrm{qr}],$ J＝[I_x，I_y，I_z，I_xz]^T′、

T′表示转置，I_x、I_y、I_z为飞行器分别绕机体坐标系的x轴、y轴和z轴方向的转动惯量，飞行器对x轴、y轴的惯性积为I_xy，对x轴、z轴的惯性积为I_xz，对y轴、z轴的惯性积为I_yz，飞行器关于机体坐标系的对称面Oxz对称，惯性积I_xy＝I_yz＝0，I_xz为非零；从t_a时刻开始到t_b时刻结束的时间段内，利用步骤(1)连续获取n组横滚角速度p、俯仰角速度q、偏航角速度r，利用步骤(3)连续获取n组滚转力矩

俯仰力矩M、偏航力矩N，利用步骤(5)连续求取n组横滚角加速度

俯仰角加速度偏航角加速度

根据得到的n组横滚角速度p、俯仰角速度q、偏航角速度r、横滚角加速度

俯仰角加速度

偏航角加速度

滚转力矩

俯仰力矩M、偏航力矩N和方程EJ＝U，利用最小二乘估计方法，求出飞行器的转动惯量I_x、I_y、I_z和惯性积I_xz，其中n＞4；

(7)根据步骤(6)得到的转动惯量I_x、I_y、I_z和惯性积I_xz，对已知的飞行器力矩方程组 $\left\{\begin{array}{c}\stackrel{\·}{p}=(c_{1}r+c_{2}p)q+c_3\stackrel{\‾}{L}+c_{4}N\\ \stackrel{\·}{q}=c_5\mathrm{pr}-c_6(p^2-r^2)+c_{7}M\\ \stackrel{\·}{r}=(c_{8}p-c_{2}r)q+c_4\stackrel{\‾}{L}+c_{9}N\end{array}\right.$ 中的9个系数c₁、c₂、c₃、c₄、c₅、c₆、c₇、c₈、c₉进行求解，其中 $c_1=\frac{(I_y-I_z)I_z-I_{\mathrm{xz}}^2}{\mathrm{\Σ}},$ $c_2=\frac{(I_x-I_y+I_z)I_{\mathrm{xz}}}{\mathrm{\Σ}},$ $c_3=\frac{I_z}{\mathrm{\Σ}},$ $c_4=\frac{I_{\mathrm{xz}}}{\mathrm{\Σ}},$ $c_5=\frac{I_z-I_x}{I_y},$ $c_6=\frac{I_{\mathrm{xz}}}{I_y},$ $c_7=\frac{1}{I_y},$ $c_8=\frac{I_x(I_x-I_y)+I_{\mathrm{xz}}^2}{\mathrm{\Σ}},$ $c_9=\frac{I_x}{\mathrm{\Σ}},$ 其中 $\mathrm{\Σ}=I_x{I}_z-I_{\mathrm{xz}}^2;$

(8)将步骤(4)得到的剩余燃油质量m_f与已知的飞行器机体质量、机载设备质量、乘员质量以及武器质量相加，得到飞行器总质量m；

(9)根据飞行动力学模型，选取飞行器的俯仰角、横滚角、横滚角速度、俯仰角速度、偏航角速度、攻角、侧滑角、真空速作为状态量，进而建立扩展卡尔曼滤波器状态方程；选取俯仰角、横滚角、横滚角速度、俯仰角速度、偏航角速度、三维线加速度以及真空速作为量测量，进而建立扩展卡尔曼滤波器量测方程；根据步骤(1)和步骤(2)获取的当前时刻即t_k+1时刻的量测信息，步骤(3)获取的上一时刻即t_k时刻的空气动力、外合力矩、发动机推力，步骤(7)求取的9个力矩方程组系数，步骤(8)得到的t_k时刻的飞行器总质量，以及t_k时刻的三维风速和三维风速变化率，利用扩展卡尔曼滤波方程得到t_k+1时刻状态量的最优估计值，从而实现大攻角飞行状态下t_k+1时刻的攻角及侧滑角的实时精确估计，其中t_k＞t_b；

(10)根据步骤(1)得到t_k+1时刻的俯仰角、横滚角、偏航角和步骤(2)得到的t_k+1时刻的三维地速参数，结合步骤(9)估计得到的t_k+1时刻的攻角、侧滑角和真空速，计算出t_k+1时刻的三维风速，并结合t_k时刻的三维风速，计算出t_k+1时刻的三维风速变化率；

(11)将步骤(10)得到的t_k+1时刻的三维风速和三维风速变化率反馈给扩展卡尔曼滤波模算法模块，用于完成步骤(9)中的下一时刻即t_k+2时刻攻角及侧滑角的估计。

所述步骤(9)中卡尔曼滤波算法的具体步骤是：

(a)扩展卡尔曼滤波器状态方程的建立

根据飞行动力学模型，选取飞行器的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、攻角α、侧滑角β、真空速V作为状态量X，即X＝[θ，φ，p，q，r，α，β，V]^T′，进而建立扩展卡尔曼滤波器的状态方程：

$\stackrel{\·}{X}\left(t\right)=f[X\left(t\right),u\left(t\right),t]+G\left(t\right)w\left(t\right)$

状态方程的具体形式为：

其中，u(t)为控制输入量，设机体坐标系下沿x轴、y轴和z轴方向的三维风速分别为u_w、v_w、w_w，三维风速u_w、v_w、w_w的变化率分别为***噪声 $w\left(t\right)={{[\mathrm{\δ}}_{\stackrel{\‾}{L}},{\mathrm{\δ}}_M,{\mathrm{\δ}}_N,{\mathrm{\δ}}_D,{\mathrm{\δ}}_Y,{\mathrm{\δ}}_L,{\mathrm{\δ}}_{T_x},{\mathrm{\δ}}_{T_y},{\mathrm{\δ}}_{T_z},{\mathrm{\δ}}_{u_w},{\mathrm{\δ}}_{v_w},{\mathrm{\δ}}_{w_w},{\mathrm{\δ}}_{{\stackrel{\·}{u}}_w},{\mathrm{\δ}}_{{\stackrel{\·}{v}}_w},{\mathrm{\δ}}_{{\stackrel{\·}{w}}_w},{\mathrm{\δ}}_m]}^{T^{\′}}$ 分别由飞行器的外合力矩

M、N、空气动力D、Y、L、发动机推力T_x、T_y、T_z、风速u_w、v_w、w_w、风速变化率

以及总质量m引起，g是重力加速度，***噪声系数阵G(t)定义为：

$G\left(t\right)=\left[\begin{array}{cccc}{0}_{2\×3}& 0_{2\×6}& 0_{2\×3}& 0_{2\×4}\\ A_{3\×3}& 0_{3\×6}& 0_{3\×3}& 0_{3\×4}\\ 0_{3\×3}& B_{3\×6}& C_{3\×3}& D_{3\×4}\end{array}\right]$

其中， $A_{3\×3}=\left[\begin{array}{ccc}{c}_3& 0& c_4\\ 0& c_7& 0\\ c_4& 0& c_9\end{array}\right]$

$B_{3\×6}=\left[\begin{array}{cccccc}0& 0& \frac{-1}{\mathrm{mV}\cos \mathrm{\β}}& \frac{-\sin \mathrm{\α}}{\mathrm{mV}\cos \mathrm{\β}}& 0& \frac{\cos \mathrm{\α}}{\mathrm{mV}\cos \mathrm{\β}}\\ 0& \frac{1}{\mathrm{mV}}& 0& \frac{-\cos \mathrm{\α}\sin \mathrm{\β}}{\mathrm{mV}}& \frac{\cos \mathrm{\β}}{\mathrm{mV}}& \frac{-\sin \mathrm{\α}\sin \mathrm{\β}}{\mathrm{mV}}\\ \frac{-1}{m}& 0& 0& \frac{\cos \mathrm{\α}\cos \mathrm{\β}}{m}& \frac{\sin \mathrm{\β}}{m}& \frac{\sin \mathrm{\α}\cos \mathrm{\β}}{m}\end{array}\right]$

$C_{3\×3}=\left[\begin{array}{ccc}\frac{q\cos \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{-p\cos \mathrm{\α}-r\sin \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{q\sin \mathrm{\α}}{V\cos \mathrm{\β}}\\ \frac{-r\cos \mathrm{\β}-q\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-r\cos \mathrm{\α}\sin \mathrm{\β}+p\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{p\cos \mathrm{\β}+q\cos \mathrm{\α}\sin \mathrm{\β}}{V}\\ -r\sin \mathrm{\β}+q\sin \mathrm{\α}\cos \mathrm{\β}& r\cos \mathrm{\α}\cos \mathrm{\β}-p\sin \mathrm{\α}\cos \mathrm{\β}& p\sin \mathrm{\β}-q\cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

$D_{3\×4}=\left[\begin{array}{cccc}\frac{\sin \mathrm{\α}}{V\cos \mathrm{\β}}& 0& \frac{-\cos \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{L+T_x\sin \mathrm{\α}-T_z\cos \mathrm{\α}}{m^{2}V\cos \mathrm{\β}}\\ \frac{\cos \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-\cos \mathrm{\β}}{V}& \frac{\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-Y+T_x\cos \mathrm{\α}\sin \mathrm{\β}-T_y\cos \mathrm{\β}+T_z\sin \mathrm{\α}\sin \mathrm{\β}}{m^{2}V}\\ -\cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}& -\sin \mathrm{\α}\cos \mathrm{\β}& \frac{D-T_x\cos \mathrm{\α}\cos \mathrm{\β}-T_y\sin \mathrm{\β}-T_z\sin \mathrm{\α}\cos \mathrm{\β}}{m^2}\end{array}\right]$

(b)扩展卡尔曼滤波器量测方程的建立

根据GPS得到的三维地速u_g、v_g、w_g，利用

解算出的V作为真空速量测量，并结合捷联惯性导航***得到的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、三维线加速度a_x、a_y、a_z组成总的量测量Z，即Z＝[θ，φ，p，q，r，a_x，a_y，a_z，V]^T′，进而建立扩展卡尔曼滤波器量测方程：

Z(t)＝h[x(t)，u(t)，t]+v(t)

量测方程具体可表示为：

$\left(\begin{array}{c}\mathrm{\θ}\\ \mathrm{\φ}\\ p\\ q\\ r\\ a_x\\ a_y\\ a_z\\ V\end{array}\right)=\left(\begin{array}{c}\mathrm{\θ}\\ \mathrm{\φ}\\ p\\ q\\ r\\ \frac{1}{m}(T_x+L\sin \mathrm{\α}-Y\cos \mathrm{\α}\sin \mathrm{\β}-D\cos \mathrm{\α}\cos \mathrm{\β})\\ \frac{1}{m}(T_y+Y\cos \mathrm{\β}-D\sin \mathrm{\β})\\ \frac{1}{m}(T_z-L\cos \mathrm{\α}-Y\sin \mathrm{\α}\sin \mathrm{\β}-D\sin \mathrm{\α}\cos \mathrm{\β})\\ V\end{array}\right)+v\left(t\right)$

其中，量测噪声 $v\left(t\right)=[{{\mathrm{\δ}}_{\mathrm{\θ}},{\mathrm{\δ}}_{\mathrm{\φ}},{\mathrm{\δ}}_p,{\mathrm{\δ}}_q,{\mathrm{\δ}}_r,{\mathrm{\δ}}_{a_x},{\mathrm{\δ}}_{a_y},{\mathrm{\δ}}_{a_z},{\mathrm{\δ}}_V]}^{T^{\′}}$ 由惯性器件的陀螺仪和线加速度计以及GPS计算的真空速的量测噪声引起；

(c)状态方程和量测方程转换为连续型线性干扰方程：

利用泰勒级数展开的方法将步骤(a)中的状态方程和步骤(b)中的量测方程转换为连续型线性干扰方程形式，即将f[X(t)，u(t)，t]和h[X(t)，u(t)，t]在状态最优估计值

处进行泰勒展开，并略去二次以上项，得到连续型线性干扰方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{X}\left(t\right)=F\left(t\right)\mathrm{\δX}\left(t\right)+G\left(t\right)w\left(t\right)\\ \mathrm{\δZ}\left(t\right)=H\left(t\right)\mathrm{\δX}\left(t\right)+v\left(t\right)\end{array}\right.$

式中， $\mathrm{\δX}\left(t\right)=[X\left(t\right)-\hat{X}\left(t\right)],$ $\mathrm{\δZ}\left(t\right)=[Z\left(t\right)-\hat{Z}\left(t\right)],$ $F\left(t\right)=\frac{\∂f[X\left(t\right),u\left(t\right),t]}{\∂X\left(t\right)}{|}_{X\left(t\right)=\hat{X}\left(t\right)},$ $H\left(t\right)=\frac{\∂h[X\left(t\right),u\left(t\right),t]}{\∂X\left(t\right)}{|}_{X\left(t\right)=\hat{X}\left(t\right)},$ $\hat{Z}\left(t\right)=h[X\left(t\right),u\left(t\right),t]{|}_{X\left(t\right)=\hat{X}\left(t\right)};$

(d)连续型线性干扰方程离散化：

取采样周期ΔT＝t_k+1-t_k对步骤(c)中得到的连续型线性干扰方程进行离散化得到离散型线性干扰方程：

$\left\{\begin{array}{c}\mathrm{\δ}{X}_{k+1}={\mathrm{\Φ}}_{k+1,k}\mathrm{\δ}{X}_k+G_k{W}_k\\ \mathrm{\δ}{Z}_{k+1}=H_{k+1}\mathrm{\δ}{X}_{k+1}+V_{k+1}\end{array}\right.$

式中， $\mathrm{\δ}{X}_{k+1}=X_{k+1}-{\hat{X}}_{k+1/k},$ $\mathrm{\δ}{Z}_{k+1}=Z_{k+1}-{\hat{Z}}_{k+1},$ 采样周期ΔT较小时Φ_k+1，k＝I+F(t_k)·ΔT， $G_k=\mathrm{\ΔT}\·(I+\frac{F\left(t_k\right)}{2!}\·\mathrm{\ΔT})\·G\left(t_k\right),$ $F\left(t_k\right)=\frac{\∂f[X\left(t_k\right),u\left(t_k\right),t_k]}{\∂X\left(t_k\right)}{|}_{X\left(t_k\right)={\hat{X}}_k},$ $H_{k+1}=\frac{\∂h[X\left(t_{k+1}\right),u\left(t_{k+1}\right),t_{k+1}]}{\∂X\left(t_{k+1}\right)}{|}_{X\left(t_{k+1}\right)={\hat{X}}_{k+1/k}},$

为上一时刻即t_k时刻的状态最优估计值，为当前时刻即t_k+1时刻状态量的一步预测值；

(e)扩展卡尔曼滤波方程：

根据步骤(3)获取的t_k时刻的空气动力D、Y、L、外合力矩

M、N、发动机推力T_x、T_y、T_z，步骤(7)求取的力矩方程组系数c₁、c₂、c₃、c₄、c₅、c₆、c₇、c₈、c₉，步骤(8)计算的t_k时刻的飞行器总质量m，以及t_k时刻的三维风速u_w、v_w、w_w和三维风速变化率

参数，并结合t_k时刻的状态最优估计值

利用

求取t_k+1时刻状态量的一步预测值

t_k+1时刻状态量的一步预测值

的方差阵P_k+1/k通过式

进行求解；t_k+1时刻滤波增益矩阵K_k+1通过式 $K_{k+1}=P_{k+1/k}{H}_{k+1}^{T^{\′}}[{H_{k+1}{P}_{k+1/k}{H}_{k+1}^{T^{\′}}+R_{k+1}]}^{-1}$ 进行求解；

根据步骤(2)获取t_k+1时刻的三维地速u_g、v_g、w_g，利用

得到t_k+1时刻真空速的量测量，并结合步骤(1)得到的t_k+1时刻俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、三维线加速度a_x、a_y、a_z参数组成t_k+1时刻总的量测量Z_k+1，结合求解得到的t_k+1时刻状态量的一步预测值

和滤波增益矩阵K_k+1，利用公式 $\mathrm{\δ}{\hat{X}}_{k+1}=K_{k+1}\{Z_{k+1}-h[{\hat{X}}_{k+1/k},u\left(t_{k+1}\right),t_{k+1}\left]\right\}$ 进行t_k+1时刻状态量改善值

的求解；

将得到的t_k+1时刻状态量改善值

和一步预测值相加，即得到t_k+1时刻的状态最优估计值

最优估计值

的误差方差阵可由公式 $P_{k+1}=(I-K_{k+1}{H}_{k+1})P_{k+1/k}{\left({I-K}_{k+1}{H}_{k+1}\right)}^{T^{\′}}+K_{k+1}{R}_{k+1}{K}_{k+1}^{T^{\′}}$ 进行求解，从而实现大攻角飞行状态下t_k+1时刻攻角及侧滑角的实时精确估计；

扩展卡尔曼滤波器中，状态量中的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q和偏航角速度r的初始值由***内部的捷联惯性导航***提供，状态量中攻角α、侧滑角β、真空速V的初始值由***外部的大气数据***提供，从而构成初始状态

初始状态估值误差方差阵P₀，***噪声方差阵初值Q₀，量测噪声方差阵初值R₀矩阵初值都由***外部直接输入，***噪声矩阵Q_k由***统噪声方差阵初值Q₀确定，量测噪声矩阵R_k+1由量测噪声方差阵初值R₀确定，另外，三维风速u_w、v_w、w_w的初始值利用***外部的风速仪提供，三维风速变化率

的初始值均取为零。

所述步骤(10)中风速及风速变化率解算的具体方法是：

根据步骤(1)得到的t_k+1时刻的俯仰角θ，横滚角φ和偏航角ψ，得到机体t_k+1时刻的姿态转换矩阵S_θψφ：

$S_{\mathrm{\θ\ψ\φ}}=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\θ}\sin \mathrm{\φ}\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right];$

结合步骤(2)获取的t_k+1时刻的三维地速u_g、v_g、w_g，得到t_k+1时刻机体坐标系下沿x轴、y轴和z轴方向的三维地速u_e，v_e，w_e为：

$\left[\begin{array}{c}{u}_e\\ v_e\\ w_e\end{array}\right]=S_{\mathrm{\θ\ψ\φ}}\left[\begin{array}{c}{u}_g\\ v_g\\ w_g\end{array}\right];$

结合步骤(9)估计得到的t_k+1时刻的攻角α、侧滑角β、真空速V参数，利用下式将真空速V转换为机体坐标系下沿x轴、y轴和z轴方向的三维真空速u_b、v_b、w_b，即

$\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]=\left[\begin{array}{c}V\cos \mathrm{\α}\cos \mathrm{\β}\\ V\sin \mathrm{\β}\\ V\sin \mathrm{\α}\cos \mathrm{\β}\end{array}\right];$

那么t_k+1时刻机体坐标系下沿x轴、y轴和z轴方向的三维风速u_w、v_w、w_w利用下式进行计算：

$\left[\begin{array}{c}{u}_w\\ v_w\\ w_w\end{array}\right]=\left[\begin{array}{c}{u}_e\\ v_e\\ w_e\end{array}\right]-\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right];$

利用计算出的t_k+1时刻的三维风速u_w(t_k+1)、v_w(t_k+1)、w_w(t_k+1)，结合t_k时刻的三维风速u_w(t_k)、v_w(t_k)、w_w(t_k)，利用微分方法求取t_k+1时刻三维风速变化率 ${\stackrel{\·}{u}}_w\left(t_{k+1}\right),{\stackrel{\·}{v}}_w\left(t_{k+1}\right),{\stackrel{\·}{w}}_w\left(t_{k+1}\right),$ 即：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_w\left(t_{k+1}\right)=\frac{u_w\left(t_{k+1}\right)-u_w\left(t_k\right)}{\mathrm{\ΔT}}\\ {\stackrel{\·}{v}}_w\left(t_{k+1}\right)=\frac{v_w\left(t_{k+1}\right)-v_w\left(t_k\right)}{\mathrm{\ΔT}}\\ {\stackrel{\·}{w}}_w\left(t_{k+1}\right)=\frac{w_w\left(t_{k+1}\right)-w_w\left(t_k\right)}{\mathrm{\ΔT}}\end{array}\right..$

本发明的优点及显著效果：本发明在不改变飞行器结构外形、不增添额外测量装置和硬件设备的前提下，充分利用现有的机载惯性导航***、GPS、飞行控制***以及燃油测量***的输出参数，解算飞行器角加速度、转动惯量、惯性积、力矩方程组的系数和总质量，基于飞行动力学模型，通过构建扩展卡尔曼滤波器，融飞行动力学模型求解和状态估计的过程为一体，实现当前时刻攻角、侧滑角和真空速的精确估计，进而解算出当前时刻的风速及风速变化率，并作为反馈信息输入到扩展卡尔曼滤波器中，完成下一时刻攻角及侧滑角的实时精确估计，通过递推求解的方式实现攻角和侧滑角的实时精确估计。本方法不仅有利于飞行器隐身，同时提高了大气数据***的测量范围和测量精度以及对复杂飞行环境的适应性，是一种相对经济且易于工程实现的方法。对于提高我国新一代航空飞行器大攻角飞行状态下攻角和侧滑角的测量范围、改善飞行器飞行性能具有重要的现实意义。

附图说明

图1是本发明方法的原理流程示意图；

图2是力矩方程组系数计算模块的原理框图；

图3是扩展卡尔曼滤波算法模块的原理框图；

图4是风速及风速变化率计算模块的原理框图；

图5是扩展卡尔曼滤波算法模块与风速及风速变化率计算模块之间信息交互原理图；

图6是本发明攻角估计效果曲线示意图；

图7是本发明侧滑角估计效果曲线示意图；

图8是本发明攻角估计误差曲线示意图；

图9是本发明侧滑角估计误差曲线示意图。

具体实施方式

参看图1-5，本发明利用现有的机载惯性导航***、GPS、飞行控制***以及燃油测量***的输出参数，解算飞行器角加速度、转动惯量、惯性积、力矩方程组的系数和总质量，基于飞行动力学模型，通过构建扩展卡尔曼滤波器，融飞行动力学模型求解和状态估计的过程为一体，实现当前时刻攻角、侧滑角和真空速的精确估计，进而解算出当前时刻的风速及风速变化率，并作为反馈信息输入到扩展卡尔曼滤波器中，完成下一时刻攻角及侧滑角的实时精确估计，通过递推求解的方式实现攻角和侧滑角的实时精确估计。

为实现大攻角飞行状态下攻角和侧滑角的估计，需要完成的工作：

(1)捷联惯性导航***数据获取：

以周期ΔT读取捷联惯性导航***中的三个角速度、三个姿态角和三个线加速度信息，三个角速度信息分别为机体坐标系下x轴、y轴和z轴方向的横滚角速度p、俯仰角速度q、偏航角速度r；三个姿态角信息分别为俯仰角θ、横滚角φ、偏航角ψ；三个线加速度信息分别为机体坐标系下x轴方向的线加速度a_x、y轴方向的线加速度a_y、z轴方向的线加速度a_z，其中机体坐标系的x轴、y轴和z轴的指向分别为向前、向右、向下；

(2)GPS***数据获取：

以周期ΔT读取GPS***输出的地理坐标系下的地速，地理坐标系方向定义为北、东、地，此三个方向的地速分别为u_g、v_g、w_g；

(3)飞行控制***数据获取：

以周期ΔT读取飞行控制***输出的飞行器所受空气动力、外合力矩、发动机推力，其中飞行器所受空气动力在气流坐标系下x_a轴负方向、y_a轴方向和z_a负方向的分量分别为阻力D、侧力Y、升力L，发动机推力在机体坐标系下x轴、y轴和z轴方向的分量分别为T_x、T_y、T_z，外合力矩在机体坐标系下x轴、y轴和z轴方向的分量分别为滚转力矩

俯仰力矩M、偏航力矩N；

(4)飞行器剩余燃油质量获取：

以周期ΔT读取飞行器燃油测量***输出的剩余燃油质量m_f；

(5)角加速度解算：

根据步骤(1)获取的机体坐标系下的横滚角速度p、俯仰角速度q、偏航角速度r，其中t时刻三个角速度信息分别为横滚角速度p(t)、俯仰角速度q(t)、偏航角速度r(t)，t+ΔT时刻三个角速度信息分别为横滚角速度p(t+ΔT)、俯仰角速度q(t+ΔT)、偏航角速度r(t+ΔT)，t时刻的机体坐标系下x轴、y轴和z轴方向的三个角加速度信息，横滚角加速度

俯仰角加速度

偏航角加速度

按微分方法计算，即：

$\left\{\begin{array}{c}\stackrel{\·}{p}\left(t\right)=\frac{p(t+\mathrm{\ΔT})-p\left(t\right)}{\mathrm{\ΔT}}\\ \stackrel{\·}{q}\left(t\right)=\frac{q(t+\mathrm{\ΔT})-q\left(t\right)}{\mathrm{\ΔT}}\\ \stackrel{\·}{r}\left(t\right)=\frac{r(t+\mathrm{\ΔT})-r\left(t\right)}{\mathrm{\ΔT}}\end{array}\right.;$

(6)飞行器转动惯量和惯性积解算：

将已知的飞行器角运动方程组 $\left\{\begin{array}{c}\stackrel{\·}{p}{I}_x-\stackrel{\·}{r}{I}_{\mathrm{xz}}+\mathrm{qr}(I_z-I_y)-\mathrm{pq}{I}_{\mathrm{xz}}=\stackrel{\‾}{L}\\ \stackrel{\·}{q}{I}_y+\mathrm{pr}(I_x-I_z)+(p^2-r^2)I_{\mathrm{xz}}=M\\ \stackrel{\·}{r}{I}_z-\stackrel{\·}{p}{I}_{\mathrm{xz}}+\mathrm{pq}(I_y-I_x)+\mathrm{qr}{I}_{\mathrm{xz}}=N\end{array}\right.$ 的三个方程式相加，得到方程EJ＝U，其中 $E=[\stackrel{\·}{p}+\mathrm{pr}-\mathrm{pq},\stackrel{\·}{q}-\mathrm{qr}+\mathrm{pq},\stackrel{\·}{r}+\mathrm{qr}-\mathrm{pr},p^2-r^2-\stackrel{\·}{p}-\stackrel{\·}{r}-\mathrm{pq}+\mathrm{qr}],$ J＝[I_x，I_y，I_z，I_xz]^T′、

T′表示转置，I_x、I_y、I_z为飞行器分别绕机体坐标系的x轴、y轴和z轴方向的转动惯量，飞行器对x轴、y轴的惯性积为I_xy，对x轴、z轴的惯性积为I_xz，对y轴、z轴的惯性积为I_yz，飞行器关于机体坐标系的对称面Oxz对称，惯性积I_xy＝I_yz＝0，I_xz为非零；从t_a时刻开始到t_b时刻结束的时间段内，利用步骤(1)连续获取n组横滚角速度p、俯仰角速度q、偏航角速度r，利用步骤(3)连续获取n组滚转力矩

俯仰力矩M、偏航力矩N，利用步骤(5)连续求取n组横滚角加速度

俯仰角加速度

偏航角加速度根据得到的n组横滚角速度p、俯仰角速度q、偏航角速度r、横滚角加速度俯仰角加速度

偏航角加速度滚转力矩

俯仰力矩M、偏航力矩N和方程EJ＝U，利用最小二乘估计方法，求出飞行器的转动惯量I_x、I_y、I_z和惯性积I_xz，其中n＞4；

(7)力矩方程组系数解算：

根据步骤(6)得到的转动惯量I_x、I_y、I_z和惯性积I_xz，对已知的飞行器力矩方程组 $\left\{\begin{array}{c}\stackrel{\·}{p}=(c_{1}r+c_{2}p)q+c_3\stackrel{\‾}{L}+c_{4}N\\ \stackrel{\·}{q}=c_5\mathrm{pr}-c_6(p^2-r^2)+c_{7}M\\ \stackrel{\·}{r}=(c_{8}p-c_{2}r)q+c_4\stackrel{\‾}{L}+c_{9}N\end{array}\right.$ 中的9个系数c₁、c₂、c₃、c₄、c₅、c₆、c₇、c₈、c₉进行求解，其中 $c_1=\frac{(I_y-I_z)I_z-I_{\mathrm{xz}}^2}{\mathrm{\Σ}},$ $c_2=\frac{(I_x-I_y+I_z)I_{\mathrm{xz}}}{\mathrm{\Σ}},$ $c_3=\frac{I_z}{\mathrm{\Σ}},$ $c_4=\frac{I_{\mathrm{xz}}}{\mathrm{\Σ}},$ $c_5=\frac{I_z-I_x}{I_y},$ $c_6=\frac{I_{\mathrm{xz}}}{I_y},$ $c_7=\frac{1}{I_y},$ $c_8=\frac{I_x(I_x-I_y)+I_{\mathrm{xz}}^2}{\mathrm{\Σ}},$ $c_9=\frac{I_x}{\mathrm{\Σ}},$ 其中 $\mathrm{\Σ}=I_x{I}_z-I_{\mathrm{xz}}^2;$

(8)飞行器总质量解算：

将步骤(4)得到的剩余燃油质量m_f与已知的飞行器机体质量、机载设备质量、乘员质量以及武器质量相加，得到飞行器总质量m

(9)扩展卡尔曼滤波算法：

根据飞行动力学模型，选取飞行器的俯仰角、横滚角、横滚角速度、俯仰角速度、偏航角速度、攻角、侧滑角、真空速作为状态量，进而建立扩展卡尔曼滤波器状态方程；选取俯仰角、横滚角、横滚角速度、俯仰角速度、偏航角速度、三维线加速度以及真空速作为量测量，进而建立扩展卡尔曼滤波器量测方程；根据步骤(1)和步骤(2)获取的当前时刻即t_k+1时刻的量测信息，步骤(3)获取的上一时刻即t_k时刻的空气动力、外合力矩、发动机推力，步骤(7)求取的9个力矩方程组系数，步骤(8)得到的t_k时刻的飞行器总质量，以及t_k时刻的三维风速和三维风速变化率，利用扩展卡尔曼滤波方程得到t_k+1时刻状态量的最优估计值，从而实现大攻角飞行状态下t_k+1时刻的攻角及侧滑角的实时精确估计，其中t_k＞t_b；

(a)扩展卡尔曼滤波器状态方程的建立

根据飞行动力学模型，选取飞行器的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、攻角α、侧滑角β、真空速V作为状态量X，即X＝[θ，φ，p，q，r，α，β，V]^T′，进而建立扩展卡尔曼滤波器的状态方程：

$\stackrel{\·}{X}\left(t\right)=f[X\left(t\right),u\left(t\right),t]+G\left(t\right)w\left(t\right)$

状态方程的具体形式为：

其中，u(t)为控制输入量，设机体坐标系下沿x轴、y轴和z轴方向的三维风速分别为u_w、v_w、w_w，三维风速u_w、v_w、w_w的变化率分别为

***噪声 $w\left(t\right)={{[\mathrm{\δ}}_{\stackrel{\‾}{L}},{\mathrm{\δ}}_M,{\mathrm{\δ}}_N,{\mathrm{\δ}}_D,{\mathrm{\δ}}_Y,{\mathrm{\δ}}_L,{\mathrm{\δ}}_{T_x},{\mathrm{\δ}}_{T_y},{\mathrm{\δ}}_{T_z},{\mathrm{\δ}}_{u_w},{\mathrm{\δ}}_{v_w},{\mathrm{\δ}}_{w_w},{\mathrm{\δ}}_{{\stackrel{\·}{u}}_w},{\mathrm{\δ}}_{{\stackrel{\·}{v}}_w},{\mathrm{\δ}}_{{\stackrel{\·}{w}}_w},{\mathrm{\δ}}_m]}^{T^{\′}}$ 分别由飞行器的外合力矩

M、N、空气动力D、Y、L、发动机推力T_x、T_y、T_z、风速u_w、v_w、w_w、风速变化率

以及总质量m引起，g是重力加速度，***噪声系数阵G(t)定义为：

$G\left(t\right)=\left[\begin{array}{cccc}{0}_{2\×3}& 0_{2\×6}& 0_{2\×3}& 0_{2\×4}\\ A_{3\×3}& 0_{3\×6}& 0_{3\×3}& 0_{3\×4}\\ 0_{3\×3}& B_{3\×6}& C_{3\×3}& D_{3\×4}\end{array}\right]$

其中， $A_{3\×3}=\left[\begin{array}{ccc}{c}_3& 0& c_4\\ 0& c_7& 0\\ c_4& 0& c_9\end{array}\right]$

$B_{3\×6}=\left[\begin{array}{cccccc}0& 0& \frac{-1}{\mathrm{mV}\cos \mathrm{\β}}& \frac{-\sin \mathrm{\α}}{\mathrm{mV}\cos \mathrm{\β}}& 0& \frac{\cos \mathrm{\α}}{\mathrm{mV}\cos \mathrm{\β}}\\ 0& \frac{1}{\mathrm{mV}}& 0& \frac{-\cos \mathrm{\α}\sin \mathrm{\β}}{\mathrm{mV}}& \frac{\cos \mathrm{\β}}{\mathrm{mV}}& \frac{-\sin \mathrm{\α}\sin \mathrm{\β}}{\mathrm{mV}}\\ \frac{-1}{m}& 0& 0& \frac{\cos \mathrm{\α}\cos \mathrm{\β}}{m}& \frac{\sin \mathrm{\β}}{m}& \frac{\sin \mathrm{\α}\cos \mathrm{\β}}{m}\end{array}\right]$

$C_{3\×3}=\left[\begin{array}{ccc}\frac{q\cos \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{-p\cos \mathrm{\α}-r\sin \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{q\sin \mathrm{\α}}{V\cos \mathrm{\β}}\\ \frac{-r\cos \mathrm{\β}-q\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-r\cos \mathrm{\α}\sin \mathrm{\β}+p\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{p\cos \mathrm{\β}+q\cos \mathrm{\α}\sin \mathrm{\β}}{V}\\ -r\sin \mathrm{\β}+q\sin \mathrm{\α}\cos \mathrm{\β}& r\cos \mathrm{\α}\cos \mathrm{\β}-p\sin \mathrm{\α}\cos \mathrm{\β}& p\sin \mathrm{\β}-q\cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

$D_{3\×4}=\left[\begin{array}{cccc}\frac{\sin \mathrm{\α}}{V\cos \mathrm{\β}}& 0& \frac{-\cos \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{L+T_x\sin \mathrm{\α}-T_z\cos \mathrm{\α}}{m^{2}V\cos \mathrm{\β}}\\ \frac{\cos \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-\cos \mathrm{\β}}{V}& \frac{\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-Y+T_x\cos \mathrm{\α}\sin \mathrm{\β}-T_y\cos \mathrm{\β}+T_z\sin \mathrm{\α}\sin \mathrm{\β}}{m^{2}V}\\ -\cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}& -\sin \mathrm{\α}\cos \mathrm{\β}& \frac{D-T_x\cos \mathrm{\α}\cos \mathrm{\β}-T_y\sin \mathrm{\β}-T_z\sin \mathrm{\α}\cos \mathrm{\β}}{m^2}\end{array}\right]$

(b)扩展卡尔曼滤波器量测方程的建立

根据GPS得到的三维地速u_g、v_g、w_g，利用

解算出的V作为真空速量测量，并结合捷联惯性导航***得到的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、三维线加速度a_x、a_y、a_z组成总的量测量Z，即Z＝[θ，φ，p，q，r，a_x，a_y，a_z，V]^T′，进而建立扩展卡尔曼滤波器量测方程：

Z(t)＝h[X(t)，u(t)，t]+v(t)

量测方程具体可表示为：

$\left(\begin{array}{c}\mathrm{\θ}\\ \mathrm{\φ}\\ p\\ q\\ r\\ a_x\\ a_y\\ a_z\\ V\end{array}\right)=\left(\begin{array}{c}\mathrm{\θ}\\ \mathrm{\φ}\\ p\\ q\\ r\\ \frac{1}{m}(T_x+L\sin \mathrm{\α}-Y\cos \mathrm{\α}\sin \mathrm{\β}-D\cos \mathrm{\α}\cos \mathrm{\β})\\ \frac{1}{m}(T_y+Y\cos \mathrm{\β}-D\sin \mathrm{\β})\\ \frac{1}{m}(T_z-L\cos \mathrm{\α}-Y\sin \mathrm{\α}\sin \mathrm{\β}-D\sin \mathrm{\α}\cos \mathrm{\β})\\ V\end{array}\right)+v\left(t\right)$

其中，量测噪声 $v\left(t\right)=[{{\mathrm{\δ}}_{\mathrm{\θ}},{\mathrm{\δ}}_{\mathrm{\φ}},{\mathrm{\δ}}_p,{\mathrm{\δ}}_q,{\mathrm{\δ}}_r,{\mathrm{\δ}}_{a_x},{\mathrm{\δ}}_{a_y},{\mathrm{\δ}}_{a_z},{\mathrm{\δ}}_V]}^{T^{\′}}$ 由惯性器件的陀螺仪和线加速度计以及GPS计算的真空速的量测噪声引起；

(c)状态方程和量测方程转换为连续型线性干扰方程：

利用泰勒级数展开的方法将步骤(a)中的状态方程和步骤(b)中的量测方程转换为连续型线性干扰方程形式，即将f[X(t)，u(t)，t]和h[X(t)，u(t)，t]在状态最优估计值

处进行泰勒展开，并略去二次以上项，得到连续型线性干扰方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{X}\left(t\right)=F\left(t\right)\mathrm{\δX}\left(t\right)+G\left(t\right)w\left(t\right)\\ \mathrm{\δZ}\left(t\right)=H\left(t\right)\mathrm{\δX}\left(t\right)+v\left(t\right)\end{array}\right.$

式中， $\mathrm{\δX}\left(t\right)=[X\left(t\right)-\hat{X}\left(t\right)],$ $\mathrm{\δZ}\left(t\right)=[Z\left(t\right)-\hat{Z}\left(t\right)],$ $F\left(t\right)=\frac{\∂f[X\left(t\right),u\left(t\right),t]}{\∂X\left(t\right)}{|}_{X\left(t\right)=\hat{X}\left(t\right)},$ $H\left(t\right)=\frac{\∂h[X\left(t\right),u\left(t\right),t]}{\∂X\left(t\right)}{|}_{X\left(t\right)=\hat{X}\left(t\right)},$ $\hat{Z}\left(t\right)=h[X\left(t\right),u\left(t\right),t]{|}_{X\left(t\right)=\hat{X}\left(t\right)};$

(d)连续型线性干扰方程离散化：

取采样周期ΔT＝t_k+1-t_k对步骤(c)中得到的连续型线性干扰方程进行离散化得到离散型线性干扰方程：

$\left\{\begin{array}{c}\mathrm{\δ}{X}_{k+1}={\mathrm{\Φ}}_{k+1,k}\mathrm{\δ}{X}_k+G_k{W}_k\\ \mathrm{\δ}{Z}_{k+1}=H_{k+1}\mathrm{\δ}{X}_{k+1}+V_{k+1}\end{array}\right.$

式中， $\mathrm{\δ}{X}_{k+1}=X_{k+1}-{\hat{X}}_{k+1/k},$ $\mathrm{\δ}{Z}_{k+1}=Z_{k+1}-{\hat{Z}}_{k+1},$ 采样周期ΔT较小时Φ_k+1，k＝I+F(t_k)·ΔT， $G_k=\mathrm{\ΔT}\·(I+\frac{F\left(t_k\right)}{2!}\·\mathrm{\ΔT})\·G\left(t_k\right),$ $F\left(t_k\right)=\frac{\∂f[X\left(t_k\right),u\left(t_k\right),t_k]}{\∂X\left(t_k\right)}{|}_{X\left(t_k\right)={\hat{X}}_k},$ $H_{k+1}=\frac{\∂h[X\left(t_{k+1}\right),u\left(t_{k+1}\right),t_{k+1}]}{\∂X\left(t_{k+1}\right)}{|}_{X\left(t_{k+1}\right)={\hat{X}}_{k+1/k}},$

为上一时刻即t_k时刻的状态最优估计值，

为当前时刻即t_k+1时刻状态量的一步预测值；

(e)扩展卡尔曼滤波方程：

根据步骤(3)获取的t_k时刻的空气动力D、Y、L、外合力矩

M、N、发动机推力T_x、T_y、T_z，步骤(7)求取的力矩方程组系数c₁、c₂、c₃、c₄、c₅、c₆、c₇、c₈、c₉，步骤(8)计算的t_k时刻的飞行器总质量m，以及t_k时刻的三维风速u_w、v_w、w_w和三维风速变化率

参数，并结合t_k时刻的状态最优估计值

利用求取t_k+1时刻状态量的一步预测值

t_k+1时刻状态量的一步预测值

的方差阵P_k+1/k通过式

进行求解；t_k+1时刻滤波增益矩阵K_k+1通过式 $K_{k+1}=P_{k+1/k}{H}_{k+1}^{T^{\′}}[{H_{k+1}{P}_{k+1/k}{H}_{k+1}^{T^{\′}}+R_{k+1}]}^{-1}$ 进行求解；

根据步骤(2)获取t_k+1时刻的三维地速u_g、v_g、w_g，利用

得到t_k+1时刻真空速的量测量，并结合步骤(1)得到的t_k+1时刻俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、三维线加速度a_x、a_y、a_z参数组成t_k+1时刻总的量测量Z_k+1，结合求解得到的t_k+1时刻状态量的一步预测值

和滤波增益矩阵K_k+1，利用公式 $\mathrm{\δ}{\hat{X}}_{k+1}=K_{k+1}\{Z_{k+1}-h[{\hat{X}}_{k+1/k},u\left(t_{k+1}\right),t_{k+1}\left]\right\}$ 进行t_k+1时刻状态量改善值的求解；

将得到的t_k+1时刻状态量改善值

和一步预测值

相加，即

得到t_k+1时刻的状态最优估计值

最优估计值

的误差方差阵可由公式 $P_{k+1}=(I-K_{k+1}{H}_{k+1})P_{k+1/k}{\left({I-K}_{k+1}{H}_{k+1}\right)}^{T^{\′}}+K_{k+1}{R}_{k+1}{K}_{k+1}^{T^{\′}}$ 进行求解，从而实现大攻角飞行状态下t_k+1时刻攻角及侧滑角的实时精确估计；

扩展卡尔曼滤波器中，状态量中的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q和偏航角速度r的初始值由***内部的捷联惯性导航***提供，状态量中攻角α、侧滑角β、真空速V的初始值由***外部的大气数据***提供，从而构成初始状态

初始状态估值误差方差阵P₀，***噪声方差阵初值Q₀，量测噪声方差阵初值R₀矩阵初值都由***外部直接输入，***噪声矩阵Q_k由***统噪声方差阵初值Q₀确定，量测噪声矩阵R_k+1由量测噪声方差阵初值R₀确定，另外，三维风速u_w、v_w、w_w的初始值利用***外部的风速仪提供，三维风速变化率

的初始值均取为零。

(10)风速及风速变化率解算：

根据步骤(1)得到t_k+1时刻的俯仰角、横滚角、偏航角和步骤(2)得到的t_k+1时刻的三维地速参数，结合步骤(9)估计得到的t_k+1时刻的攻角、侧滑角和真空速，计算出t_k+1时刻的三维风速，并结合t_k时刻的三维风速，计算出t_k+1时刻的三维风速变化率；

根据步骤(1)得到的t_k+1时刻的俯仰角θ，横滚角φ和偏航角ψ，得到机体t_k+1时刻的姿态转换矩阵S_θψφ：

$S_{\mathrm{\θ\ψ\φ}}=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\θ}\sin \mathrm{\φ}\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right];$

结合步骤(2)获取的t_k+1时刻的三维地速u_g、v_g、w_g，得到t_k+1时刻机体坐标系下沿x轴、y轴和z轴方向的三维地速u_e，v_e，w_e为：

$\left[\begin{array}{c}{u}_e\\ v_e\\ w_e\end{array}\right]=S_{\mathrm{\θ\ψ\φ}}\left[\begin{array}{c}{u}_g\\ v_g\\ w_g\end{array}\right];$

结合步骤(9)估计得到的t_k+1时刻的攻角α、侧滑角β、真空速V参数，利用下式将真空速V转换为机体坐标系下沿x轴、y轴和z轴方向的三维真空速u_b、v_b、w_b，即

$\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]=\left[\begin{array}{c}V\cos \mathrm{\α}\cos \mathrm{\β}\\ V\sin \mathrm{\β}\\ V\sin \mathrm{\α}\cos \mathrm{\β}\end{array}\right];$

那么t_k+1时刻机体坐标系下沿x轴、y轴和z轴方向的三维风速u_w、v_w、w_w利用下式进行计算：

$\left[\begin{array}{c}{u}_w\\ v_w\\ w_w\end{array}\right]=\left[\begin{array}{c}{u}_e\\ v_e\\ w_e\end{array}\right]-\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right];$

利用计算出的t_k+1时刻的三维风速u_w(t_k+1)、v_w(t_k+1)、w_w(t_k+1)，结合t_k时刻的三维风速u_w(t_k)、v_w(t_k)、w_w(t_k)，利用微分方法求取t_k+1时刻三维风速变化率 ${\stackrel{\·}{u}}_w\left(t_{k+1}\right),{\stackrel{\·}{v}}_w\left(t_{k+1}\right),{\stackrel{\·}{w}}_w\left(t_{k+1}\right),$ 即：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_w\left(t_{k+1}\right)=\frac{u_w\left(t_{k+1}\right)-u_w\left(t_k\right)}{\mathrm{\ΔT}}\\ {\stackrel{\·}{v}}_w\left(t_{k+1}\right)=\frac{v_w\left(t_{k+1}\right)-v_w\left(t_k\right)}{\mathrm{\ΔT}}\\ {\stackrel{\·}{w}}_w\left(t_{k+1}\right)=\frac{w_w\left(t_{k+1}\right)-w_w\left(t_k\right)}{\mathrm{\ΔT}}\end{array}\right..$

(11)风速和风速变化率反馈步骤：

将步骤(10)得到的t_k+1时刻的三维风速和三维风速变化率反馈给扩展卡尔曼滤波模算法模块，用于完成步骤(9)中的下一时刻即t_k+2时刻攻角及侧滑角的估计。

因此，通过递推求解的方式可以实现攻角和侧滑角的实时精确估计。

图1是本发明方法的基本原理框图，攻角/侧滑角计算***利用捷联惯性导航***、GPS***、飞行控制***和燃油测量***提供的参数，通过力矩方程组系数计算模块、风速及风速变化率计算模块、总质量计算模块以及扩展卡尔曼滤波算法模块，最终实现攻角和侧滑角的精确估计，其中各个模块之间的实线箭头代表了基本的逻辑连接关系。

图2是对图1中的力矩方程组系数计算模块的进一步细化，代表了力矩方程组系数计算的具体流程，更清晰的表明了信息的来源和传递过程。

图3是对图1中的扩展卡尔曼滤波算法模块的进一步细化，代表了攻角、侧滑角及真空速计算的具体流程，更清晰的表明了信息的来源和扩展卡尔曼滤波器的主要构成。

图4是对图1中的风速及风速变化率计算模块的进一步细化，更清晰的表明了当前时刻风速及风速变化率的计算流程和信息的来源。

图5是对图1中的扩展卡尔曼滤波算法模块和风速及风速变化率计算模块之间的信息交互的进一步细化，更清晰的表明了两个模块之间信息的在不同时刻相互传递和利用的过程，其中

为状态量的初始值，t₁为扩展卡尔曼滤波的开始时刻，

是t₁时刻的状态量的估计值，t₂是t₁的下一时刻，

是t₂时刻的状态量的估计值，

是t_k+1时刻的状态量的估计值，

是t_k+2时刻的状态量的估计值。

图6是本发明方法攻角估计效果曲线，横轴代表时间，纵轴表示攻角，飞行攻角变化范围-41°～75°，利用该发明估计的攻角与真实攻角曲线基本重合，吻合度较好，应用该发明不仅能够实现常规飞行攻角下时对攻角的精确估计，而且对大攻角飞行具有较好的跟踪效果。

图7是本发明方法侧滑角估计效果曲线，横轴代表时间，纵轴表示侧滑角，侧滑角变化范围为-12°～22°，利用该发明估计的侧滑角与真实侧滑角曲线基本重合，应用该发明能够对大攻角飞行状态下的侧滑角进行精确估计。

图8是本发明方法攻角估计误差曲线，横轴代表时间，纵轴代表攻角估计误差，应用该发明所估计的攻角误差均值为0.31°，估计攻角误差的标准差为0.20°，攻角估计最大误差控制在1.2°以内。

图9是本发明方法侧滑角估计误差曲线，横轴代表时间，纵轴代表侧滑角的估计误差，应用该发明所估计的侧滑角误差均值为0.05°，估计侧滑角误差的标准差为0.33°，侧滑角估计误差控制在1.2°以内。

具体实施例

本实施例基于飞行模拟软件X-Plane仿真的飞行数据，对大攻角飞行状态下的攻角和侧滑角估计方法进行实验，得出了有益的结论。

基于X-Plane***的仿真试验效果：从攻角和侧滑角估计效果曲线图6和图7及估计误差曲线图8和图9可以看出，本发明不仅能够在常规飞行时实现攻角和侧滑角的精确估计，而且能够对大攻角飞行具有较好的跟踪效果，其中攻角和侧滑角估计误差均值分别为0.31°和0.05°，估计误差的标准差分别为0.20°和0.33°，尽管在大攻角飞行阶段估计的攻角和侧滑角误差略有增大，但是所估计的攻角和侧滑角误差均能控制在1.2°以内。本发明具有很强的工程应用价值。

Claims (4)

1.一种大攻角飞行状态下的大气攻角及侧滑角估计方法，其特征在于：利用飞行器机载设备输出的参数，包括：

①捷联惯性导航***输出的姿态角、角速度以及线加速度；

②GPS***输出的地速；

③飞行控制***输出的空气动力、外合力矩、发动机推力；

④燃油测量***输出的剩余燃油质量；

由角速度通过微分方法求解出角加速度；

由角速度、角加速度和外合力矩，通过最小二乘估计方法求解出飞行器的转动惯量和惯性积，进而得到力矩方程组的系数；

由剩余燃油质量和已知的飞行器机体质量、机载设备质量、乘员质量以及武器质量相加，得到飞行器总质量；

根据飞行动力学模型，构建扩展卡尔曼滤波器，利用扩展卡尔曼滤波算法，实现大攻角飞行状态下当前时刻攻角、侧滑角和真空速的精确估计；

由当前时刻的姿态角、地速以及估计出的当前时刻的攻角、侧滑角、真空速，对当前时刻的风速进行解算，并结合上一时刻的风速，对当前时刻的风速变化率进行解算；

将得到的当前时刻的风速和风速变化率反馈给扩展卡尔曼滤波模算法模块，用于完成下一时刻的攻角及侧滑角的实时精确估计，通过递推求解的方式实现攻角和侧滑角的实时精确估计。

2.根据权利要求1所述的大攻角飞行状态下的大气攻角及侧滑角估计方法，包括以下步骤：

（1）以周期ΔT读取捷联惯性导航***中的三个角速度、三个姿态角和三个线加速度信息，三个角速度信息分别为机体坐标系下x轴、y轴和z轴方向的横滚角速度p、俯仰角速度q、偏航角速度r；三个姿态角信息分别为俯仰角θ、横滚角φ、偏航角ψ；三个线加速度信息分别为机体坐标系下x轴方向的线加速度a_x、y轴方向的线加速度a_y、z轴方向的线加速度a_z，其中机体坐标系的x轴、y轴和z轴的指向分别为向前、向右、向下；

（2）以周期ΔT读取GPS***输出的地理坐标系下的地速，地理坐标系方向定义为北、东、地，此三个方向的地速分别为u_g、v_g、w_g；

（3）以周期ΔT读取飞行控制***输出的飞行器所受空气动力、外合力矩、发动机推力，其中飞行器所受空气动力在气流坐标系下x_a轴负方向、y_a轴方向和z_a轴负方向的分量分别为阻力D、侧力Y、升力L，发动机推力在机体坐标系下x轴、y轴和z轴方向的分量分别为T_x、T_y、T_z，外合力矩在机体坐标系下x轴、y轴和z轴方向的分量分别为滚转力矩

俯仰力矩M、偏航力矩N；

（4）以周期ΔT读取飞行器燃油测量***输出的剩余燃油质量m_f；

（5）根据步骤（1）获取的机体坐标系下的横滚角速度p、俯仰角速度q、偏航角速度r，其中t时刻三个角速度信息分别为横滚角速度p(t)、俯仰角速度q(t)、偏航角速度r(t)，t+ΔT时刻三个角速度信息分别为横滚角速度p(t+ΔT)、俯仰角速度q(t+ΔT)、偏航角速度r(t+ΔT)，t时刻的机体坐标系下x轴、y轴和z轴方向的三个角加速度信息，即横滚角加速度

俯仰角加速度

偏航角加速度按微分方法计算，即：

$\left\{\begin{array}{c}\stackrel{\·}{p}\left(t\right)=\frac{p(t+\mathrm{\ΔT})-p\left(t\right)}{\mathrm{\ΔT}}\\ \stackrel{\·}{q}\left(t\right)=\frac{q(t+\mathrm{\ΔT})-q\left(t\right)}{\mathrm{\ΔT}}\\ \stackrel{\·}{r}\left(t\right)=\frac{r(t+\mathrm{\ΔT})-r\left(t\right)}{\mathrm{\ΔT}}\end{array}\right.;$

（6）将已知的飞行器角运动方程组 $\left\{\begin{array}{c}\stackrel{\·}{p}{I}_x-\stackrel{\·}{r}{I}_{\mathrm{xz}}+\mathrm{qr}(I_z-I_y)-\mathrm{pq}{I}_{\mathrm{xz}}=\stackrel{\‾}{L}\\ \stackrel{\·}{q}{I}_y+\mathrm{pr}(I_x-I_z)+(p^2-r^2)I_{\mathrm{xz}}=M\\ \stackrel{\·}{r}{I}_z-\stackrel{\·}{p}{I}_{\mathrm{xz}}+\mathrm{pq}(I_y-I_x)+\mathrm{qr}{I}_{\mathrm{xz}}=N\end{array}\right.$ 的三个方程式相加，得到方程EJ=U，其中 $E=[\stackrel{\·}{p}+\mathrm{pr}-\mathrm{pq},\stackrel{\·}{q}-\mathrm{qr}+\mathrm{pq},\stackrel{\·}{r}+\mathrm{qr}-\mathrm{pr},p^2-r^2-\stackrel{\·}{p}-\stackrel{\·}{r}-\mathrm{pq}+\mathrm{qr}],$ J=[I_x,I_y,I_z,I_xz]^T'、

T'表示转置，I_x、I_y、I_z为飞行器分别绕机体坐标系的x轴、y轴和z轴方向的转动惯量，飞行器对x轴、y轴的惯性积为I_xy，对x轴、z轴的惯性积为I_xz，对y轴、z轴的惯性积为I_yz，飞行器关于机体坐标系的对称面Oxz对称，惯性积I_xy=I_yz=0，I_xz为非零；从t_a时刻开始到t_b时刻结束的时间段内，利用步骤（1）连续获取n组横滚角速度p、俯仰角速度q、偏航角速度r，利用步骤（3）连续获取n组滚转力矩

俯仰力矩M、偏航力矩N，利用步骤（5）连续求取n组横滚角加速度

俯仰角加速度

偏航角加速度

根据得到的n组横滚角速度p、俯仰角速度q、偏航角速度r、横滚角加速度俯仰角加速度

偏航角加速度滚转力矩

俯仰力矩M、偏航力矩N和方程EJ=U，利用最小二乘估计方法，求出飞行器的转动惯量I_x、I_y、I_z和惯性积I_xz，其中n>4；

（7）根据步骤（6）得到的转动惯量I_x、I_y、I_z和惯性积I_xz,对已知的飞行器力矩方程组 $\left\{\begin{array}{c}\stackrel{\·}{p}=(c_{1}r+c_{2}p)q+c_3\stackrel{\‾}{L}+c_{4}N\\ \stackrel{\·}{q}=c_5\mathrm{pr}-c_6(p^2-r^2)+c_{7}M\\ \stackrel{\·}{r}=(c_{8}p-c_{2}r)q+c_4\stackrel{\‾}{L}+c_{9}N\end{array}\right.$ 中的9个系数c₁、c₂、c₃、c₄、c₅、c₆、c₇、c₈、c₉进行求解，其中 $c_1=\frac{(I_y-I_z)I_z-I_{\mathrm{xz}}^2}{\mathrm{\Σ}},$ $c_2=\frac{(I_x-I_y+I_z)I_{\mathrm{xz}}}{\mathrm{\Σ}},$ $c_3=\frac{I_z}{\mathrm{\Σ}},$ $c_4=\frac{I_{\mathrm{xz}}}{\mathrm{\Σ}},$ $c_5=\frac{I_z-I_x}{I_y},$ $c_6=\frac{I_{\mathrm{xz}}}{I_y},$ $c_7=\frac{1}{I_y},$ $c_8=\frac{I_x(I_x-I_y)+I_{\mathrm{xz}}^2}{\mathrm{\Σ}},$ $c_9=\frac{I_x}{\mathrm{\Σ}},$ 其中 $\mathrm{\Σ}=I_x{I}_z-I_{\mathrm{xz}}^2;$

（8）将步骤（4）得到的剩余燃油质量m_f与已知的飞行器机体质量、机载设备质量、乘员质量以及武器质量相加，得到飞行器总质量m；

（9）根据飞行动力学模型，选取飞行器的俯仰角、横滚角、横滚角速度、俯仰角速度、偏航角速度、攻角、侧滑角、真空速作为状态量，进而建立扩展卡尔曼滤波器状态方程；选取俯仰角、横滚角、横滚角速度、俯仰角速度、偏航角速度、三维线加速度以及真空速作为量测量，进而建立扩展卡尔曼滤波器量测方程；根据步骤（1）和步骤（2）获取的当前时刻即t_k+1时刻的量测信息，步骤（3）获取的上一时刻即t_k时刻的空气动力、外合力矩、发动机推力，步骤（7）求取的9个力矩方程组系数，步骤（8）得到的t_k时刻的飞行器总质量，以及t_k时刻的三维风速和三维风速变化率，利用扩展卡尔曼滤波方程得到t_k+1时刻状态量的最优估计值，从而实现大攻角飞行状态下t_k+1时刻的攻角及侧滑角的实时精确估计，其中t_k>t_b；

（10）根据步骤（1）得到t_k+1时刻的俯仰角、横滚角、偏航角和步骤（2）得到的t_k+1时刻的三维地速参数，结合步骤（9）估计得到的t_k+1时刻的攻角、侧滑角和真空速，计算出t_k+1时刻的三维风速，并结合t_k时刻的三维风速，计算出t_k+1时刻的三维风速变化率；

（11）将步骤（10）得到的t_k+1时刻的三维风速和三维风速变化率反馈给扩展卡尔曼滤波模算法模块，用于完成步骤（9）中的下一时刻即t_k+2时刻攻角及侧滑角的估计。

3.根据权利要求2所述的大攻角飞行状态下的大气攻角及侧滑角估计方法，其特征在于：所述步骤（9）中扩展卡尔曼滤波算法的具体步骤是：

（a）扩展卡尔曼滤波器状态方程的建立

根据飞行动力学模型，选取飞行器的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、攻角α、侧滑角β、真空速V作为状态量X，即X=[θ,φ,p,q,r,α,β,V]^T'，进而建立扩展卡尔曼滤波器的状态方程：

$\stackrel{\·}{X}\left(t\right)=f[X\left(t\right),u\left(t\right),t]+G\left(t\right)w\left(t\right)$

状态方程的具体形式为：

$\left(\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{p}\\ \stackrel{\·}{q}\\ \stackrel{\·}{r}\\ \stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ \stackrel{\·}{V}\end{array}\right)=\left(\begin{array}{c}q\cos \mathrm{\φ}-r\sin \mathrm{\φ}\\ p+(r\cos \mathrm{\φ}+q\sin \mathrm{\φ})\tan \mathrm{\θ}\\ (c_{1}r+c_{2}p)q+c_3\stackrel{\‾}{L}+c_{4}N\\ c_5\mathrm{pr}-c_6(p^2-r^2)+c_{7}M\\ (c_{8}p-c_{2}r)q+c_4\stackrel{\‾}{L}+c_{9}N\\ \frac{1}{\mathrm{mV}\cos \mathrm{\β}}[-T_x\sin \mathrm{\α}+T_z\cos \mathrm{\α}-L+\mathrm{mV}(-p\cos \mathrm{\α}\sin \mathrm{\β}+q\cos \mathrm{\β}-r\sin \mathrm{\α}\sin \mathrm{\β})+\\ \mathrm{mg}(\sin \mathrm{\α}\sin \mathrm{\θ}+\cos \mathrm{\α}\cos \mathrm{\φ}\cos \mathrm{\θ})+m({\mathrm{qw}}_w-{\mathrm{rv}}_w+{\stackrel{\·}{u}}_w)\sin \mathrm{\α}-m({\mathrm{pv}}_w-{\mathrm{qu}}_w+{\stackrel{\·}{w}}_w)\cos \mathrm{\α}]\\ \frac{1}{\mathrm{mV}}[-T_x\cos \mathrm{\α}\sin \mathrm{\β}+T_y\cos \mathrm{\β}-T_z\sin \mathrm{\α}\sin \mathrm{\β}+Y-\mathrm{mV}(-p\sin \mathrm{\α}+r\cos \mathrm{\α})+\\ \mathrm{mg}(\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\θ}+\cos \mathrm{\β}\sin \mathrm{\φ}\cos \mathrm{\θ}-\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\φ}\cos \mathrm{\θ})+\\ m({\mathrm{qw}}_w-{\mathrm{rv}}_w+{\stackrel{\·}{u}}_w)\cos \mathrm{\α}\sin \mathrm{\β}+m({\mathrm{pw}}_w-{\mathrm{ru}}_w-{\stackrel{\·}{v}}_w)\cos \mathrm{\β}+m({\mathrm{pv}}_w-{\mathrm{qu}}_w+{\stackrel{\·}{w}}_w)\sin \mathrm{\α}\sin \mathrm{\β}]\\ \frac{1}{m}[T_x\cos \mathrm{\α}\cos \mathrm{\β}+T_y\sin \mathrm{\β}+T_z\sin \mathrm{\α}\cos \mathrm{\β}-D+\mathrm{mg}(-\cos \mathrm{\α}\cos \mathrm{\β}\sin \mathrm{\θ}+\\ \sin \mathrm{\β}\sin \mathrm{\φ}\cos \mathrm{\θ}+\sin \mathrm{\α}\cos \mathrm{\β}\cos \mathrm{\φ}\cos \mathrm{\θ})-m({\mathrm{qw}}_w-{\mathrm{rv}}_w+{\stackrel{\·}{u}}_w)\cos \mathrm{\α}\cos \mathrm{\β}+\\ m({\mathrm{pw}}_w-{\mathrm{ru}}_w-{\stackrel{\·}{v}}_w)\sin \mathrm{\β}-m({\mathrm{pv}}_w-{\mathrm{qu}}_w+{\stackrel{\·}{w}}_w)\sin \mathrm{\α}\cos \mathrm{\β}]\end{array}\right)+G\left(t\right)w\left(t\right)$

其中，u(t)为控制输入量，设机体坐标系下沿x轴、y轴和z轴方向的三维风速分别为u_w、v_w、w_w，三维风速u_w、v_w、w_w的变化率分别为

***噪声 $w\left(t\right)={[{\mathrm{\δ}}_{\stackrel{\‾}{L}},{\mathrm{\δ}}_M,{\mathrm{\δ}}_N,{\mathrm{\δ}}_D,{\mathrm{\δ}}_Y,{\mathrm{\δ}}_L,{\mathrm{\δ}}_{T_x},{\mathrm{\δ}}_{T_y},{\mathrm{\δ}}_{T_z},{\mathrm{\δ}}_{u_w},{\mathrm{\δ}}_{v_w},{\mathrm{\δ}}_{w_w},{\mathrm{\δ}}_{{\stackrel{.}{u}}_w},{\mathrm{\δ}}_{{\stackrel{.}{v}}_w},{\mathrm{\δ}}_{{\stackrel{.}{w}}_w},{\mathrm{\δ}}_m]}^{T^{\′}}$ 分别由飞行器的外合力矩M、N、空气动力D、Y、L、发动机推力T_x、T_y、T_z、风速u_w、v_w、w_w、风速变化率

以及总质量m引起，g是重力加速度，***噪声系数阵G(t)定义为：

$G\left(t\right)=\left[\begin{array}{cccc}{0}_{2\×3}& 0_{2\×6}& 0_{2\×3}& 0_{2\×4}\\ A_{3\×3}& 0_{3\×6}& 0_{3\×3}& 0_{3\×4}\\ 0_{3\×3}& B_{3\×6}& C_{3\×3}& D_{3\×4}\end{array}\right]$

其中， $A_{3\×3}=\left[\begin{array}{ccc}{c}_3& 0& c_4\\ 0& c_7& 0\\ c_4& 0& c_9\end{array}\right]$

$B_{3\×6}=\left[\begin{array}{cccccc}0& 0& \frac{-1}{\mathrm{mV}\cos \mathrm{\β}}& \frac{-\sin \mathrm{\α}}{\mathrm{mV}\cos \mathrm{\β}}& 0& \frac{\cos \mathrm{\α}}{\mathrm{mV}\cos \mathrm{\β}}\\ 0& \frac{1}{\mathrm{mV}}& 0& \frac{-\cos \mathrm{\α}\sin \mathrm{\β}}{\mathrm{mV}}& \frac{\cos \mathrm{\β}}{\mathrm{mV}}& \frac{-\sin \mathrm{\α}\sin \mathrm{\β}}{\mathrm{mV}}\\ \frac{-1}{m}& 0& 0& \frac{\cos \mathrm{\α}\cos \mathrm{\β}}{m}& \frac{\sin \mathrm{\β}}{m}& \frac{\sin \mathrm{\α}\cos \mathrm{\β}}{m}\end{array}\right]$

$C_{3\×3}\left[\begin{array}{ccc}\frac{q\cos \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{-p\cos \mathrm{\α}-r\sin \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{q\sin \mathrm{\α}}{V\cos \mathrm{\β}}\\ \frac{-r\cos \mathrm{\β}-q\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-r\cos \mathrm{\α}\sin \mathrm{\β}+p\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{p\cos \mathrm{\β}+q\cos \mathrm{\α}\sin \mathrm{\β}}{V}\\ -r\sin \mathrm{\β}+q\sin \mathrm{\α}\cos \mathrm{\β}& r\cos \mathrm{\α}\cos \mathrm{\β}-p\sin \mathrm{\α}\cos \mathrm{\β}& p\sin \mathrm{\β}-q\cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

$D_{3\×4}=\left[\begin{array}{cccc}\frac{\sin \mathrm{\α}}{V\cos \mathrm{\β}}& 0& \frac{-\cos \mathrm{\α}}{V\cos \mathrm{\β}}& \frac{L+T_x\sin \mathrm{\α}-T_z\cos \mathrm{\α}}{m^{2}V\cos \mathrm{\β}}\\ \frac{\cos \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-\cos \mathrm{\β}}{V}& \frac{\sin \mathrm{\α}\sin \mathrm{\β}}{V}& \frac{-Y+T_x\cos \mathrm{\α}\sin \mathrm{\β}-T_y\cos \mathrm{\β}+T_z\sin \mathrm{\α}\sin \mathrm{\β}}{m^{2}V}\\ -\cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\β}& -\sin \mathrm{\α}\cos \mathrm{\β}& \frac{D-T_x\cos \mathrm{\α}\cos \mathrm{\β}-T_y\sin \mathrm{\β}-T_z\sin \mathrm{\α}\cos \mathrm{\β}}{m^2}\end{array}\right]$

（b）扩展卡尔曼滤波器量测方程的建立

根据GPS得到的三维地速u_g、v_g、w_g，利用解算出的V作为真空速量测量，并结合捷联惯性导航***得到的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、三维线加速度a_x、a_y、a_z组成总的量测量Z，即Z=[θ,φ,p,q,r,a_x,a_y,a_z,V]^T'，进而建立扩展卡尔曼滤波器量测方程：

Z(t)=h[X(t),u(t),t]+v(t)

量测方程具体可表示为：

$\left(\begin{array}{c}\mathrm{\θ}\\ \mathrm{\φ}\\ p\\ q\\ r\\ a_x\\ a_y\\ a_z\\ V\end{array}\right)=\left(\begin{array}{c}\mathrm{\θ}\\ \mathrm{\φ}\\ p\\ q\\ r\\ \frac{1}{m}(T_x+L\sin \mathrm{\α}-Y\cos \mathrm{\α}\sin \mathrm{\β}-D\cos \mathrm{\α}\cos \mathrm{\β})\\ \frac{1}{m}(T_y+Y\cos \mathrm{\β}-D\sin \mathrm{\β})\\ \frac{1}{m}(T_z-L\cos \mathrm{\α}-Y\sin \mathrm{\α}\sin \mathrm{\β}-D\sin \mathrm{\α}\cos \mathrm{\β})\\ V\end{array}\right)+v\left(t\right)$

其中，量测噪声

由惯性器件的陀螺仪和线加速度计以及GPS计算的真空速的量测噪声引起；

（c）状态方程和量测方程转换为连续型线性干扰方程：

利用泰勒级数展开的方法将步骤（a）中的状态方程和步骤（b）中的量测方程转换为连续型线性干扰方程形式，即将f[X(t),u(t),t]和h[X(t),u(t),t]在状态最优估计值

处进行泰勒展开，并略去二次以上项，得到连续型线性干扰方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{X}\left(t\right)=F\left(t\right)\mathrm{\δX}\left(t\right)+G\left(t\right)w\left(t\right)\\ \mathrm{\δZ}\left(t\right)=H\left(t\right)\mathrm{\δX}\left(t\right)+v\left(t\right)\end{array}\right.$

式中， $\mathrm{\δX}\left(t\right)=[X\left(t\right)-\hat{X}\left(t\right)],$ $\mathrm{\δZ}\left(t\right)=[Z\left(t\right)-\hat{Z}\left(t\right)],$ $F\left(t\right)={\frac{\∂f[X\left(t\right),u\left(t\right),t]}{\∂X\left(t\right)}|}_{X\left(t\right)=\hat{X}\left(t\right)},$ $H\left(t\right)={\frac{\∂h[X\left(t\right),u\left(t\right),t]}{\∂X\left(t\right)}|}_{X\left(t\right)=\hat{X}\left(t\right)},$ $\hat{Z}\left(t\right)=h[X\left(t\right),u\left(t\right),t]{|}_{X\left(t\right)=\hat{X}\left(t\right)};$

（d）连续型线性干扰方程离散化：

取采样周期ΔT=t_k+1-t_k对步骤（c）中得到的连续型线性干扰方程进行离散化得到离散型线性干扰方程：

$\left\{\begin{array}{c}\mathrm{\δ}{X}_{k+1}={\mathrm{\Φ}}_{k+1,k}{\mathrm{\δX}}_k+G_k{W}_k\\ \mathrm{\δ}{Z}_{k+1}=H_{k+1}\mathrm{\δ}{X}_{k+1}+V_{k+1}\end{array}\right.$

式中， ${\mathrm{\δX}}_{k+1}=X_{k+1}-{\hat{X}}_{k+1/k},$ ${\mathrm{\δZ}}_{k+1}=Z_{k+1}-{\hat{Z}}_{k+1},$ 采样周期ΔT较小时Φ_k+1,k=I+F(t_k)·ΔT， $G_k=\mathrm{\ΔT}\·(I+\frac{F\left(t_k\right)}{2!}\·\mathrm{\ΔT})\·G\left(t_k\right),$ $F\left(t_k\right)={\frac{\∂f[X\left(t_k\right),u\left(t_k\right),t_k]}{\∂X\left(t_k\right)}|}_{X\left(t_k\right)={\hat{X}}_k},$

为上一时刻即t_k时刻的状态最优估计值，

为当前时刻即t_k+1时刻状态量的一步预测值；

（e）扩展卡尔曼滤波方程：

根据步骤（3）获取的t_k时刻的空气动力D、Y、L、外合力矩

M、N、发动机推力T_x、T_y、T_z，步骤（7）求取的力矩方程组系数c₁、c₂、c₃、c₄、c₅、c₆、c₇、c₈、c₉，步骤（8）计算的t_k时刻的飞行器总质量m，以及t_k时刻的三维风速u_w、v_w、w_w和三维风速变化率

参数，并结合t_k时刻的状态最优估计值

利用

求取t_k+1时刻状态量的一步预测值

t_k+1时刻状态量的一步预测值

的方差阵P_k+1/k通过式

进行求解；t_k+1时刻滤波增益矩阵K_k+1通过式 $K_{k+1}=P_{k+1/k}{H}_{k+1}^{T^{\′}}{[H_{k+1}{P}_{k+1/k}{H}_{k+1}^{T^{\′}}+R_{k+1}]}^{-1}$ 进行求解；

根据步骤（2）获取t_k+1时刻的三维地速u_g、v_g、w_g，利用

得到t_k+1时刻真空速的量测量，并结合步骤（1）得到的t_k+1时刻俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q、偏航角速度r、三维线加速度a_x、a_y、a_z参数组成t_k+1时刻总的量测量Z_k+1，结合求解得到的t_k+1时刻状态量的一步预测值和滤波增益矩阵K_k+1，利用公式 $\mathrm{\δ}{\hat{X}}_{k+1}=K_{k+1}\{Z_{k+1}-h[{\hat{X}}_{k+1/k},u\left(t_{k+1}\right),t_{k+1}\left]\right\}$ 进行t_k+1时刻状态量改善值的求解；

将得到的t_k+1时刻状态量改善值

和一步预测值

相加，即

得到t_k+1时刻的状态最优估计值

最优估计值

的误差方差阵可由公式 $P_{k+1}=(I-K_{k+1}{H}_{k+1})P_{k+1/k}{(I-K_{k+1}{H}_{k+1})}^{T^{\′}}+K_{k+1}{R}_{k+1}{K}_{k+1}^{T^{\′}}$ 进行求解，从而实现大攻角飞行状态下t_k+1时刻攻角及侧滑角的实时精确估计；

扩展卡尔曼滤波器中，状态量中的俯仰角θ、横滚角φ、横滚角速度p、俯仰角速度q和偏航角速度r的初始值由***内部的捷联惯性导航***提供，状态量中攻角α、侧滑角β、真空速V的初始值由***外部的大气数据***提供，从而构成初始状态

初始状态估值误差方差阵P₀，***噪声方差阵初值Q₀，量测噪声方差阵初值R₀矩阵初值都由***外部直接输入，***噪声矩阵Q_k由***统噪声方差阵初值Q₀确定，量测噪声矩阵R_k+1由量测噪声方差阵初值R₀确定，另外，三维风速u_w、v_w、w_w的初始值利用***外部的风速仪提供，三维风速变化率

的初始值均取为零。

4.根据权利要求3所述的大攻角飞行状态下的大气攻角及侧滑角估计方法，其特征在于：所述步骤（10）中风速和风速变化率解算的具体方法是：

根据步骤（1）得到的t_k+1时刻的俯仰角θ，横滚角φ和偏航角ψ，得到机体t_k+1时刻的姿态转换矩阵S_θψφ：

$S_{\mathrm{\θ\ψ\φ}}=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\θ}\sin \mathrm{\φ}\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]\text{;}$

结合步骤（2）获取的t_k+1时刻的三维地速u_g、v_g、w_g，得到t_k+1时刻机体坐标系下沿x轴、y轴和z轴方向的三维地速u_e,v_e,w_e为：

$\left[\begin{array}{c}{u}_e\\ v_e\\ w_e\end{array}\right]=S_{\mathrm{\θ\ψ\φ}}\left[\begin{array}{c}{u}_g\\ v_g\\ w_g\end{array}\right];$

结合步骤（9）估计得到的t_k+1时刻的攻角α、侧滑角β、真空速V参数，利用下式将真空速V转换为机体坐标系下沿x轴、y轴和z轴方向的三维真空速u_b、v_b、w_b，即

$\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]=\left[\begin{array}{c}V\cos \mathrm{\α}\cos \mathrm{\β}\\ V\sin \mathrm{\β}\\ V\sin \mathrm{\α}\cos \mathrm{\β}\end{array}\right];$

那么t_k+1时刻机体坐标系下沿x轴、y轴和z轴方向的三维风速u_w、v_w、w_w利用下式进行计算：

$\left[\begin{array}{c}{u}_w\\ v_w\\ w_w\end{array}\right]=\left[\begin{array}{c}{u}_e\\ v_e\\ w_e\end{array}\right]-\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right];$

利用计算出的t_k+1时刻的三维风速u_w(t_k+1)、v_w(t_k+1)、w_w(t_k+1)，结合t_k时刻的三维风速u_w(t_k)、v_w(t_k)、w_w(t_k)，利用微分方法求取t_k+1时刻三维风速变化率

即：

$\left\{\begin{array}{c}{\stackrel{.}{u}}_w\left(t_{k+1}\right)=\frac{u_w\left(t_{k+1}\right)-u_w\left(t_k\right)}{\mathrm{\ΔT}}\\ {\stackrel{.}{v}}_w\left(t_{k+1}\right)=\frac{v_w\left(t_{k+1}\right)-v_w\left(t_k\right)}{\mathrm{\ΔT}}\\ {\stackrel{.}{w}}_w\left(t_{k+1}\right)=\frac{w_w\left(t_{k+1}\right)-w_w\left(t_k\right)}{\mathrm{\ΔT}}\end{array}\right..$

Images