一种无人机空速估计和空速管故障检测方法与流程

本发明涉及无人机用空速估计和空速管故障检测方法，属于航空传感器故障检测领域，应用于采用多传感器(IMU，GPS，大气数据机，空速管)的无人机平台，能快速检测空速管发生的故障。
背景技术：
：无人机由于其经济性和机动性优势受到越来越广泛的应用。无人机是一个典型的多传感器系统，包括IMU，GPS，空速管，磁罗盘等传感器，这些传感器为无人机的飞行控制和导航提供重要信息，因此，传感器的可靠性是保证无人机正常飞行的基础。无人机空速的测量对于无人机飞行至关重要，错误的空速信息会输出错误的飞行包络线，从而导致无人机坠毁。常规意义下的空速测量是利用空速管测量飞行动压，大气数据计算机将其转换为速度信号。而比起相对可靠的电路系统，空速管经常会受到异物堵塞或结冰影响而不能正常输出动压信息，从而导致空速输出故障。因此，检测并且隔离空速管的故障对于无人机飞行至关重要。对商用大型飞机而言，通常解决这一问题的方法是采取硬件冗余技术，即多个空速管冗余使用提高整体的可靠性。然而，基于无人机的尺寸以及经济性考虑，此方案并不是最佳选择。因此，利用基于解析冗余的故障检测方法是无人机空速管故障检测的有效方案。2012年IEEEWorkshopEESMS会议，M.Fravllini等人的论文“Modelbasedapproachesfortheairspeedestimationandfaultmonitoringofanunmannedaerialvehicles”利用攻角模型估计空速，并通过与空速管的测量值比较来检测空速管是否发生故障。但是此方法要求准确的无人机模型，即具体的空气动力参数，因此针对不同的无人机需要修改模型参数，方法使用受限。2011年IEEEAerospaceandElectronicSystems期刊，A.Cho等人的论文“WindestimationandairspeedcalibrationusingaUAVwithasingle-antennaGPSreceiverandpitottube”利用空速三角的几何关系，并且融合了GPS和空速管输出信息，估计出空速并检测空速管故障。但是此方法在估计的过程中使用了空速管的信息，因此无法保证故障信号的独立性。现有的基于解析冗余的空速管故障检测方法都是利用无人机空气动力模型，其模型中的参数需要通过辨识获得，对于不同型号的无人机使用受限。同时，为了获得准确的估计结果，常常利用空速管的信息作为量测量，因此会导致故障信号与正常输出信号分辨困难。技术实现要素：本发明解决的问题是：无人机在飞行的过程中，空速管容易受异物堵塞或结冰而出现故障，通常的硬件冗余技术在在重量轻、结构简单的无人机平台使用困难，而基于无人机动力学模型的解析冗余故障检测方法又依赖特定的无人机型号，从而限制其适用范围。因此建立不依赖特定尺寸和型号无人机的模型估计空速，同时估计空速与空速管输出信息独立，并通过与空速管输出比较来检测空速管故障。本发明的技术解决方案为：一种无人机空速估计和空速管故障检测方法，包括以下步骤：步骤1：定义载体坐标系oxbybzb和北东地导航坐标系oxnynzn。步骤2：无人机飞控系统采集各传感器输出信息：IMU输出角速率ω和加速度信息a，GPS输出速度信息vgps，INS/GPS组合导航系统输出姿态信息迎角/侧滑角传感器输出风角信息α与β，空速管输出空速信息Vm。步骤3：利用IMU输出解算无人机姿态信息同时融合GPS输出信息对姿态信息进行估计补偿。步骤4：以角速率ω、加速度a以及姿态信息为系统输入，建立状态方程。步骤5：以GPS速度和迎角α、侧滑角β为量测值，建立量测方程。步骤6：根据上述空速估计模型，利用EKF估计无人机的空速步骤7：获取估计空速与空速管量测空速Vm之间的残差r，利用累加图检测统计量是否超出阈值，并由此检测空速管的故障。其中，步骤1所述的载体坐标系oxbybzb和北东地导航坐标系oxnynzn分别定义为：载体坐标系oxbybzb定义：原点o位于飞机的质心，oxb轴与载体运动方向的重心线重合，正向指向载体的运动方向，oyb轴垂直oxb轴指向载体的右侧，ozb轴与oxb轴、oyb轴正交形成右手坐标系。北东地导航坐标系oxnynzn定义：原点与载体坐标系的原点重合以消除坐标原点的漂移，oxn轴指向当地北子午线，oyn轴与oxn轴垂直指向东，ozn轴与oxn轴、oyn轴垂直形成右手坐标系向下。步骤2所述的无人机飞控系统采集各传感器输出信息为：(1)IMU输出三轴角速率ω和三轴加速度a：ω＝[ωxωyωz]Ta＝[axayaz]T其中下标x,y,z表示向量在载体坐标系三轴上的投影。(2)GPS输出绝对速度其中vn、ve、vd分别表示北向、东向和地向速度，上标n表示在导航坐标系的投影。(3)迎角/侧滑角传感器输出风角信息α与β分别为：迎角α计算方法：α=Pα1-Pα2K1(P3-P4)]]>侧滑角β计算方法：β=Pβ1-Pβ2K1(P5-P6)]]>式中P3表示迎角探针左压力，P4为右压力，Pα1为上压力，Pα2为下压力；P5表示侧滑角探针上压力，P6为下压力，Pβ1为左压力，Pβ2为右压力；K1表示转换系数；(4)根据空速管测量的压力信息计算量测空速Vm假设空速管外的气流不可压缩，并且依据伯努利方程可以计算得到风速的量测值为：Vm=2(P0-P)/ρ]]>其中P0，P分别表示空速管测得的全压和静压，ρ表示空气密度。步骤3所述的利用IMU输出解算无人机姿态信息同时融合GPS输出信息对姿态信息进行估计补偿的具体步骤为：a.根据速度微分方程、位置微分方程以及姿态微分方程实时解算INS系统的导航信息；速度微分方程：v·n=Cbna-(2ωien+ωenn)×vn+g]]>位置微分方程：L·=vnRM+hλ·=ve(RN+h)cosLh·=-vd]]>其中vn＝[vnvevd]T表示绝对速度，下标n，e，d分别表示绝对速度向量在北向，动向和地向的投影；a为加速度计输出，为载体系b系到导航系n系的姿态转移矩阵，为地球自转角速率在北东地导航坐标系下的投影，为北东地导航坐标系系相对地球坐标系的转动角速率在北东地导航坐标系下的投影，且有：[Lλh]T表示位置向量，L,λ,h分别为北东地导航坐标系的纬度、经度、高度；RN为卯酉圈曲率半径，Re为WGS84大地坐标系地球参考椭球的赤道平面半径，e为WGS84大地坐标系地球参考椭球的椭圆度；RM为子午圈曲率半径，g＝[00g]T，g表示重力加速度。姿态微分方程：C·bn=Cbnωnbb]]>其中：ωnbb=ωibb-Cnb(ωien+ωenn)]]>为载体坐标系b系到导航坐标系n系的方向余弦矩阵：Cnb=cos(ψ)sin(ψ)0-sin(ψ)cos(ψ)00011000cos(θ)sin(θ)0-sin(θ)cos(θ)cos(φ)0-sin(φ)010sin(φ)0cos(φ)]]>其中φ，θ，ψ表示姿态角在北东地导航坐标系内三个方向的投影，为的转置矩阵，为陀螺仪的输出，利用四阶龙格库塔捷联导航解算算法计算新周期的速度、位置和姿态信息。b.利用卡尔曼滤波模块解算INS/GPS组合导航系统的导航信息。组合导航系统的状态方程为：X·=AX+GW]]>其中：X=(δΦn)T(δvn)T(δpn)TϵT▿TT]]>δΦn，δvn，δpn表示平台误差角、速度误差以及位置误差，上标n表示三个误差向量在北东地导航坐标系的投影；ε＝[εxεyεz]T表示三轴陀螺常值漂移，▽＝[▽x▽y▽z]T表示三轴加计常值漂移，G表示系统的噪声驱动阵，W表示系统的噪声矢量，上标T表示矩阵的转置矩阵。量测方程为：Z＝HX+V其中：Z＝[δvnTδpnT]Tδvn=vn-vgpsnδpn=pn-pgpsn]]>H=03×3I3×303×303×303×303×303×3I3×303×303×3]]>在建立了系统方程和量测方程后，将系统方程和量测方程离散化，再利用卡尔曼滤波器进行滤波。利用卡尔曼滤波器进行滤波时，四阶龙格库塔捷联解算模块的输出周期为Ts，GPS接收机的输出周期为Tgps，卡尔曼滤波的离散化周期为Tgps；当没有GPS接收机信息的输出时，只进行INS导航结算；在接收到卫星接收机信息时，利用GPS与IMU的输出进行量测更新。c.利用估计得到的位置误差δpn、速度误差δvn和姿态误差δΦn补偿模对INS系统的导航解算结果进行补偿：vn＝vn-δvnpn＝pn-δpnCnb=Cnb·Cnt]]>Cnt=cos(δψ)sin(δψ)0-sin(δψ)cos(δψ)00011000cos(δθ)sin(δθ)0-sin(δθ)cos(δθ)cos(δφ)0-sin(δφ)010sin(δφ)0cos(δφ)]]>最后可得到校正后的姿态信息步骤4所述的以角速率ω、加速度a以及姿态信息为系统输入，建立状态方程的具体步骤为：空速微分方程：u·(t)v·(t)w·(t)=Cnb(t)00g+ax(t)ay(t)az(t)+wa(t)+0-w(t)v(t)w(t)0-u(t)-v(t)u(t)0(ωx(t)ωy(t)ωz(t)+wω(t))]]>风速微分方程：μ·n(t)μ·e(t)μ·d(t)=wμ(t)]]>其中表示无人机空速在载体系三个轴方向上的投影，μ(t)＝[μn(t)μe(t)μd(t)]T表示风速在导航系三个轴方向上的投影，wa(t)，wω(t)，wμ(t)分别表示加速度计，陀螺仪和风速的噪声矢量。根据上述微分方程建立空速状态方程：x·(t)=fc(x(t),u(t),w(t))]]>其中：x(t)＝[u(t)v(t)w(t)μn(t)μe(t)μd(t)]Tu(t)＝[ωx(t)ωy(t)ωz(t)ax(t)ay(t)az(t)]Tw(t)＝[wω(t)Twa(t)Twμ(t)T]T步骤5所述的以GPS速度和迎角α、侧滑角β为量测值，建立量测方程的具体步骤为：利用GPS接收机输出的绝对速度与大气数据机输出的迎角/侧滑角α(t)，β(t)作为量测量：vgpsn(t)=vn(t)ve(t)vd(t)=Cbn(t)u(t)v(t)w(t)+μn(t)μe(t)μd(t)]]>α(t)=tan-1(w(t)u(t))]]>β(t)=sin-1(v(t)u(t)2+v(t)2+w(t)2)]]>建立量测方程：y(t)＝h(x(t))+d(t)其中：d(t)＝[dgps(t)Tdα(t)dβ(t)]T表示GPS速度、迎角和侧滑角噪声。对状态方程和量测方程进行离散化，离散时间为Tgps：xk＝xk-1+Tgpsfc(xk-1,uk-1,wk-1)＝f(xk-1,uk-1,wk-1)yk＝y(kTgps)＝h(xk)+dk步骤6所述的利用EKF估计无人机的空速的步骤为：初值设置：表示初值的估计值，P0表示误差协方差矩阵的初值。噪声方差阵根据各传感器的规格参数设置：Qk和Rk分别表示过程噪声和量测噪声的协方差矩阵。时间更新：x^k,k-1=f(x^k-1,k-1,uk-1,0)]]>Pk,k-1=Fk-1Pk-1,k-1Fk-1T+Lk-1Qk-1Lk-1T]]>和Pk,k-1分别表示状态和误差协方差矩阵的一步预测值。其中Fk-1和Lk-1分别表示转移矩阵对状态和误差雅克比矩阵。量测更新：Kk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1]]>x^k,k=x^k,k-1+Kk(yk-h(x^k,k-1))]]>Pk,k=(I-KkHk)Pk,k-1(I-KkHk)T+KkRkKkT]]>其中：Hk=∂hk∂x|x^k,k-1]]>Kk表示卡尔曼滤波的增益，表示k时刻的状态滤波结果，Pk,k表示k时刻误差协方差矩阵的滤波结果。步骤7所述的空速管故障判断方法为：k时刻残差r(k)由第k时刻的估计空速与空速管量测空速Vm(k)做差所得：r(k)=Vm(k)-|V^(k)|=Vm(k)-(u^(k))2+(v^(k))2+(w^(k))2]]>基于残差的累加图统计量为：S+(k)=sup(0,S+(k-1)-τ(τ-2r(k))2σ02),S+(1)=0]]>其中σ0表示残差r的标准差，τ表示故障检验的敏感参数，表示故障幅度的期望值，通常基于实际情况考虑取τ＝σ0。针对上述统计量，将检测阈值设计为最大统计量的1.5倍，即：Sth＝1.5max(S+(k))，k＝1,2,3.....一旦统计量S+(k)超过阈值Sth，则认为发生故障。本发明的原理：本发明针对无人机空速管故障检测问题，提出一种基于估计空速与真实空速管输出作比较的方法来确定故障的发生。空速的估计使用了IMU测量的加速度和角速率作为方程输入，GPS测量的速度位置、迎角/侧滑角传感器测量的风角信息作为方程输出，以空速三个方向的量作为状态，并且基于无人机空速三角几何关系对空速进行估计。所得的估计值与空速管信息独立，且估计过程与无人机空气动力参数无关。基于此，将估计值与空速管信息做差得到空速残差，利用累加图对空速管进行故障检测。本发明与现有技术相比的优点在于：(1)本发明利用IMU量测信息建立空速状态方程，利用GPS和迎角/侧滑角传感器信息估计空速，不同于一般的空速估计方法基于无人机的空气动力学模型，需要对应的空气动力参数。因此本发明适用范围不局限于某一型号的无人机，具有广泛的使用价值。(2)本发明中的空速估计过程是利用IMU输出，GPS输出以及迎角/侧滑角传感器输出，而空速管的输出信息所自带的干扰以及故障不会影响空速的估计过程，因此相比于一般基于空速管输出信息的空速估计方法，本方法具有较高的精度。附图说明图1为本发明的无人机空速估计和空速管故障检测方法步骤图；图2为“托尔”无人机第一次飞行试验结果展示，其中，图2a为“托尔”无人机第一次飞行轨迹；图2b为“托尔”无人机第一次飞行的估计空速与量测空速图；图3为“托尔”无人机第二次飞行试验结果展示，其中，图3a为“托尔”无人机第二次飞行轨迹；图3b为“托尔”无人机第二次飞行的估计空速与量测空速图；图4为“费舍尔”无人机第一次飞行试验结果展示，其中，图4a为“费舍尔”无人机第一次飞行轨迹；图4b为“费舍尔”无人机第一次飞行的估计空速与量测空速图；图5为“托尔”无人机第一次飞行空速管故障检测结果；图6为“托尔”无人机第二次飞行空速管故障检测结果；图7为“费舍尔”无人机第一次飞行空速管故障检测结果。具体实施方式如图1所示，本发明的具体实施包括以下步骤：步骤1：定义载体坐标系oxbybzb和北东地导航坐标系oxnynzn。载体坐标系oxbybzb定义：原点o位于飞机的质心，oxb轴与载体运动方向的重心线重合，正向指向载体的运动方向，oyb轴垂直oxb轴指向载体的右侧，ozb轴与oxb轴、oyb轴正交形成右手坐标系，以下简称为b系。北东地导航坐标系oxnynzn定义：原点与载体坐标系的原点重合以消除坐标原点的漂移，oxn轴指向当地北子午线，oyn轴与oxn轴垂直指向东，ozn轴与oxn轴、oyn轴垂直形成右手坐标系向下，以下简称为n系。步骤2：无人机飞控系统采集各传感器输出信息：(1)IMU输出三轴角速率ω和三轴加速度a：ω＝[ωxωyωz]Ta＝[axayaz]T其中下标x,y,z表示向量在载体坐标系三轴上的投影。(2)GPS输出绝对速度其中vn、ve、vd分别表示北向、东向和地向速度，上标n表示在导航坐标系的投影。(3)迎角/侧滑角传感器输出风角信息α与β分别为：迎角α计算方法：α=Pα1-Pα2K1(P3-P4)]]>侧滑角β计算方法：β=Pβ1-Pβ2K1(P5-P6)]]>式中P3表示迎角探针左压力，P4为右压力，Pα1为上压力，Pα2为下压力；P5表示侧滑角探针上压力，P6为下压力，Pβ1为左压力，Pβ2为右压力；K1表示转换系数；(4)根据空速管测量的压力信息计算量测空速Vm假设空速管外的气流不可压缩，并且依据伯努利方程可以计算得到风速的量测值为：Vm=2(P0-P)/ρ]]>其中P0，P分别表示空速管测得的全压和静压，ρ表示空气密度。步骤3：利用IMU输出解算无人机姿态信息同时融合GPS输出信息对姿态信息进行估计补偿。a.根据速度微分方程、位置微分方程以及姿态微分方程实时解算INS系统的导航信息；速度微分方程：v·n=Cbna-(2ωien+ωenn)×vn+g]]>位置微分方程：L·=vnRM+hλ·=ve(RN+h)cosLh·=-vd]]>其中vn＝[vnvevd]T表示绝对速度，下标n，e，d表示绝对速度向量在北东地导航坐标系的投影；a为加速度计输出，为载体系b系到导航系n系的姿态转移矩阵，为地球自转角速率在北东地导航坐标系下的投影，为北东地导航坐标系系相对地球坐标系的转动角速率在北东地导航坐标系下的投影，且有：[Lλh]T表示位置向量，L,λ,h分别为北东地导航坐标系的纬度、经度、高度；RN为卯酉圈曲率半径，Re为WGS84大地坐标系地球参考椭球的赤道平面半径，e为WGS84大地坐标系地球参考椭球的椭圆度；RM为子午圈曲率半径，g＝[00g]T，g表示重力加速度。姿态微分方程：C·bn=Cbnωnbb]]>其中ωnbb=ωibb-Cnb(ωien+ωenn)]]>为载体坐标系b系到导航坐标系n系的方向余弦矩阵：Cnb=cos(ψ)sin(ψ)0-sin(ψ)cos(ψ)00011000cos(θ)sin(θ)0-sin(θ)cos(θ)cos(φ)0-sin(φ)010sin(φ)0cos(φ)]]>其中φ，θ，ψ表示姿态角在北东地导航坐标系内三个方向的投影，为的转置矩阵，为陀螺仪的输出，利用四阶龙格库塔捷联导航解算算法计算新周期的速度、位置和姿态信息。b.利用卡尔曼滤波模块解算INS/GPS组合导航系统的导航信息。组合导航系统的状态方程为：X·=AX+GW]]>其中X＝[δΦnTδvnTδpnTεT▽T]TδΦn，δvn，δpn表示平台误差角、速度误差以及位置误差，上标n表示在北东地导航坐标系的投影；ε＝[εxεyεz]T表示三轴陀螺常值漂移，▽＝[▽x▽y▽z]T表示三轴加计常值漂移，G表示系统的噪声驱动阵，W表示系统的噪声矢量，上标T表示矩阵的转置矩阵。量测方程为：Z＝HX+V其中Z＝[δvnTδpnT]Tδvn=vn-vgpsnδpn=pn-pgpsn]]>H=03×3I3×303×303×303×303×303×3I3×303×303×3]]>在建立了系统方程和量测方程后，将系统方程和量测方程离散化，再利用卡尔曼滤波器进行滤波。利用卡尔曼滤波器进行滤波时，四阶龙格库塔捷联解算模块的输出周期为Ts，GPS接收机的输出周期为Tgps，卡尔曼滤波的离散化周期为Tgps；当没有GPS接收机信息的输出时，只进行INS导航结算；在接收到卫星接收机信息时，利用GPS与IMU的输出进行量测更新。c.利用估计得到的位置误差δpn、速度误差δvn和姿态误差δΦn补偿模对INS系统的导航解算结果进行补偿：vn＝vn-δvnpn＝pn-δpnCnb=Cnb·Cnt]]>Cnt=cos(δψ)sin(δψ)0-sin(δψ)cos(δψ)00011000cos(δθ)sin(δθ)0-sin(δθ)cos(δθ)cos(δφ)0-sin(δφ)010sin(δφ)0cos(δφ)]]>最后可得到校正后的姿态信息步骤4：以角速率ω、加速度a以及姿态信息为系统输入，建立状态方程。空速微分方程：u·(t)v·(t)w·(t)=Cnb(t)00g+ax(t)ay(t)az(t)+wa(t)+0-w(t)v(t)w(t)0-u(t)-v(t)u(t)0(ωx(t)ωy(t)ωz(t)+wω(t))]]>风速微分方程：μ·n(t)μ·e(t)μ·d(t)=wμ(t)]]>其中表示无人机空速在载体系三个轴方向上的投影，μ(t)＝[μn(t)μe(t)μd(t)]T表示风速在导航系三个轴方向上的投影，wa(t)，wω(t)，wμ(t)分别表示加速度计，陀螺仪和风速的噪声矢量。根据上述微分方程建立空速状态方程：x·(t)=fc(x(t),u(t),w(t))]]>其中：x(t)＝[u(t)v(t)w(t)μn(t)μe(t)μd(t)]Tu(t)＝[ωx(t)ωy(t)ωz(t)ax(t)ay(t)az(t)]Tw(t)＝[wω(t)Twa(t)Twμ(t)T]T步骤5：以GPS速度和迎角α、侧滑角β为量测值，建立量测方程。利用GPS接收机输出的绝对速度与大气数据机输出的迎角/侧滑角α(t)，β(t)作为量测量：vgpsn(t)=vn(t)ve(t)vd(t)=Cbn(t)u(t)v(t)w(t)+μn(t)μe(t)μd(t)]]>α(t)=tan-1(w(t)u(t))]]>β(t)=sin-1(v(t)u(t)2+v(t)2+w(t)2)]]>建立量测方程：y(t)＝h(x(t))+d(t)其中：d(t)＝[dgps(t)Tdα(t)dβ(t)]T表示GPS速度、迎角和侧滑角噪声。对状态方程和量测方程进行离散化，离散时间为Tgps：xk＝xk-1+Tgpsfc(xk-1,uk-1,wk-1)＝f(xk-1,uk-1,wk-1)yk＝y(kTgps)＝h(xk)+dk步骤6：根据上述空速估计模型，利用EKF估计无人机的空速初值设置：表示初值的估计值，P0表示误差协方差矩阵的初值。噪声方差阵根据各传感器的规格参数设置：Qk和Rk分别表示过程噪声和量测噪声的协方差矩阵。时间更新：x^k,k-1=f(x^k-1,k-1,uk-1,0)]]>Pk,k-1=Fk-1Pk-1,k-1Fk-1T+Lk-1Qk-1Lk-1T]]>和Pk,k-1分别表示状态和误差协方差矩阵的一步预测值。其中Fk-1和Lk-1分别表示转移矩阵对状态和误差雅克比矩阵。量测更新：Kk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1]]>x^k,k=x^k,k-1+Kk(yk-h(x^k,k-1))]]>Pk,k=(I-KkHk)Pk,k-1(I-KkHk)T+KkRkKkT]]>其中Hk=∂hk∂x|x^k,k-1]]>Kk表示卡尔曼滤波的增益，表示k时刻的状态滤波结果，Pk,k表示k时刻误差协方差矩阵的滤波结果。步骤7：获取估计空速与空速管量测空速Vm之间的残差r，利用累加图检测统计量超出阈值，并由此判断空速管的故障。k时刻残差r(k)由第k时刻的估计空速与空速管量测空速Vm(k)做差所得：r(k)=Vm(k)-|V^(k)|=Vm(k)-(u^(k))2+(v^(k))2+(w^(k))2]]>基于残差的累加图统计量为：S+(k)=sup(0,S+(k-1)-τ(τ-2r(k))2σ02),S+(1)=0]]>其中σ0表示残差r的标准差，τ表示故障检验的敏感参数，表示故障幅度的期望值，通常基于实际情况考虑取τ＝σ0。针对上述统计量，将检测阈值设计为最大统计量的1.5倍，即：Sth＝1.5max(S+(k))，k＝1,2,3.....一旦统计量S+(k)超过阈值Sth，则认为发生故障。实施例以明尼苏达大学无人机实验室的UltraStick25e无人机平台实际飞行数据进行空速估计和空速管故障检测。平台包含三架无人机，其中选择“托尔”型和“费舍尔”号作为实施对象。两架无人机除了尺寸和重量略有差别以外，都具有相同的传感器配置，包括GloasatEM-406A型GPS接收机，ADIS16405型IMU以及Goodrich公司的微型五孔探针。按照说明书步骤1定义无人机的载体坐标系和导航坐标系。载体坐标系oxbybzb定义：原点o位于飞机的质心，oxb轴与载体运动方向的重心线重合，正向指向载体的运动方向，oyb轴垂直oxb轴指向载体的右侧，ozb轴与oxb轴、oyb轴正交形成右手坐标系。北东地导航坐标系oxnynzn定义：原点与载体坐标系的原点重合以消除坐标原点的漂移，oxn轴指向当地北子午线，oyn轴与oxn轴垂直指向东，ozn轴与oxn轴、oyn轴垂直形成右手坐标系向下。按照说明书步骤2采集IMU，GPS，五孔探针以及空速管的输出信息，传感器精度如下表所示：表1无人机传感器输出误差按照说明书步骤3，根据IMU输出信息以及GPS输出信息估计并校正姿态信息。按照说明书步骤4，以角速率，加速度以及姿态信息为系统输入，建立空速状态方程。按照说明书步骤5，以GPS速度，迎角以及侧滑角信息为量测量，建立量测方程。噪声方差阵为：Qk=Q=diagσax2σay2σaz2σωx2σωy2σωz2σμx2σμy2σμz2]]>Rk=R=diagσα2σβ2σvx2σvy2σvz2]]>按照说明书步骤6，根据空速模型，利用EKF估计空速。试验选用两次“托尔”型无人机飞行数据和一次“费舍尔”型无人机飞行数据进行空速估计，飞行轨迹和空速估计结果如图2至图4所示。其中图2a表示第一次“托尔”型无人机飞行轨迹，图2b表示第一次“托尔”型无人机飞行的估计空速和量测空速，其中实线表示估计空速，虚线表示空速管量测空速，单位为m/s，横坐标是数据处理时间，间隔为0.02s；图3a表示第二次“托尔”型无人机飞行轨迹，图3b表示第二次“托尔”型无人机飞行的估计空速和量测空速，其中实线表示估计空速，虚线表示空速管量测空速，单位为m/s，横坐标是数据处理时间，间隔为0.02s；图4a表示第一次“费舍尔”型无人机飞行轨迹，图4b表示第一次“费舍尔”型无人机飞行的估计空速和量测空速，其中实线表示估计空速，虚线表示空速管量测空速，单位为m/s，横坐标是数据处理时间，间隔为0.02s；从三次飞行试验结果可以看出，本发明可以在没有空速管信息的情况下准确的估计出空速信息，尤其是针对不同型号的无人机，仍然可以准确的估计出空速，统计结果如下表所示：表2无人机飞行试验统计结果结果表明，空速估计的误差方差可以保持在较低的水平内。按照说明书步骤7,对空速管故障进行检测。由于飞行试验是在无故障条件下进行的，因此需要对空速管输出信息进行故障注入，下表表示三个架次飞行数据故障注入信息：表3无人机空速管故障注入信息无人机型号飞行架次故障幅值(m/s)故障发生时间(k)阈值托尔10.601518000～18500335托尔20.65996000～6500139费舍尔10.788513000～13500322故障检测结果如图5至图7所示。图5表示第一次“托尔”无人机飞行试验的累加图统计量，虚线表示无故障时的累加图统计量，实线表示有故障时的累加图统计量，横线表示预设阈值，横坐标表示数据处理时间，间隔为0.02s，在k＝18000～18500时，有故障注入，统计量为730，超出预设阈值335，表示检测到故障发生；图6表示第二次“托尔”无人机飞行试验的累加图统计量虚线表示无故障时的累加图统计量，实线表示有故障时的累加图统计量，横线表示预设阈值，横坐标表示数据处理时间，间隔为0.02s，在k＝6000～6500时，有故障注入，统计量为342，超出预设阈值139，表示检测到故障发生；图7表示第一次“费舍尔”无人机飞行试验的累加图统计量，虚线表示无故障时的累加图统计量，实线表示有故障时的累加图统计量，横线表示预设阈值，横坐标表示数据处理时间，间隔为0.02s，在k＝13000～13500时，有故障注入，统计量为479，超出预设阈值322，表示检测到故障发生；从上述三次飞行实验看，在故障注入时刻以及持续时间内，累加图统计量都超过了提前设定的阈值，提示故障发生，因此本发明有效的检测了故障的发生。当前第1页1 2 3