专利名称：高空长航无人机的多传感器容错自主导航方法
技术领域：
发明涉及一种用于高空长航无人 机的多传感器容错自主导航方法，属于航空飞行 器组合导航技术领域，可应用于高空长时间飞行的航空飞行器导航参数的确定，适用于高 空长时间飞行的航空飞行器的导航定位。
背景技术：
星敏感器作为一种高精度的天文姿态敏感器，越来越广泛应用于自主导航领域。 星敏感器可以在不需要任何外部基准信息的前提下，直接精确测量获得飞行器相对于惯性 坐标系下的姿态信息，且其测量精度在导航全程保持稳定，现有最高精度可以达到角秒级。 惯性/天文组合导航系统以其可以全天候工作，具有极强的自主性和导航误差不随时间发 散而在高空、长航飞行器上得到了极大重视和发展，但在现有技术条件下，由于天文导航定 位受到水平姿态基准的影响，因此，惯性/天文组合导航精度还无法达到惯性/GPS组合导 航定位精度。合成孔径雷达(SAR)是二战以后发展起来的一种高分辨率成像雷达，利用它可以 全天候、全天时、远距离的得到类似光学照相的高分辨率雷达图像。用实时SAR图像进行图 像匹配定位并和惯导组合构成INS/SAR组合导航系统在近年来得到了发展和广泛重视。但 由于SAR成像导航系统在载机应用环境下具有其特殊要求，如SAR成像时一般要求载机处 于比较平稳的飞行状态；进行SAR图像匹配时需要耗用不等的匹配计算时间；此外，由于受 到机载数字地图数据库容量的限制，SAR图像导航仅能在部分时段进行工作，目前多用于末 端制导。惯性/GPS组合导航系统具有覆盖范围广、全天候、全天时、高精度和近乎连续实 时等特点，用户设备具有体积小、重量轻、功耗小和易于操作等优点。因此惯性/GPS组合导 航系统是一种比较理想的导航系统，目前应用最广泛。但是GPS信号易受电磁干扰，从而导 致INS/GPS定位精度下降；而且GPS隶属于美国，一旦出现战争情况，GPS信号将完全丢失， 此时INS/GPS组合导航系统中只有INS发挥作用，误差会不断地积累，影响飞机的导航精 度。因此，现有的航空机载惯性/GPS组合的自主导航方法存在可靠性不高和导航精 度不稳地性，不能充分满足高空长航无人机对导航精度稳定性的要求。

发明内容
本发明目的在于克服航空机载惯性/GPS组合导航系统的导航稳定性的不足，提 供了一种基于天文、SAR辅助的惯性/GPS多传感器容错自主导航方法即高空长航无人机的 多传感器容错自主导航方法。本发明为实现上述目的，采用如下技术方案本发明基于天文、SAR辅助的惯性/GPS多传感器容错自主导航方法，包括以下步 骤
(1)通过建立航空机载惯性导航系统INS的误差状态量方程，得到了对航空机载惯性导航系统误差状态量的数学描述，航空机载惯性导航系统误差状态量X定义为X =\ΦΕ,ΦΝ,Φυ,δΛ>ε,δνΝ,δνυ’δΙ^δλ,Sh,EbxJbyJbzJrx,εσ,ε—ν y,N J，φΕ, φΝ, (J)u 分别表
示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量 和天向平台误差角状态量；S νΕ，δνΝ, δ Vu分别表示航空机载惯性导航系统误差状态量中 的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；S L，δ λ, 5h分别 表示航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差 状态量；￡bx，￡by，￡bz，ε 分别表示航空机载惯性导航系统误差状态量中的X 轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移 误差状态量；Vjc，分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴和Z轴方 向加速度计零偏，T为转置；(2)采用航空机载地理系下位置线性化观测原理，建立航空机载地理系下位置观 测量和被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的经纬高误差状态 量之间的线性化量测方程，包括GPS/INS量测方程，CNS/气压高度表/INS量测方程和SAR/ INS量测方程；(3)子系统采用残差X 2检验法进行故障检测和隔离；(4)将步骤(2)所述的纬度、经度和高度误差状态量进行卡尔曼滤波，同时对子系 统观测量和状态量进行故障检测，当子系统无故障时，将子系统卡尔曼滤波结果送入联邦 滤波器；当子系统有故障时，子系统将被隔离，卡尔曼滤波结果也不能送入联邦滤波器，其 他无故障子系统构成新的联邦滤波系统。(5)联邦滤波器对子系统送来的数据进行数据融合，输出全局最优估计值，从而对 航空机载惯性导航系统的导航误差进行修正。本发明与现有技术相比克服了航空机载惯性/GPS组合导航系统的导航稳定性的 不足，构建了一种适用于高空长航时无人机的多传感器容错自主导航方法，它具有以下优 点(1)每个局部滤波器互不影响，只是对自己的子系统进行检测。(2)当GPS信息受到干 扰时，故障检测单元能够及时检测出故障，从而将GPS子系统隔离开联邦滤波系统；(3)当 故障传感器被隔离后，其他局部滤波器仍能继续正常工作，剩余的子系统重新构成新的联 邦滤波系统，不影响主滤波器的工作，便于系统重建。


图1为本发明的多传感器容错自主导航方法的一种实施例子的流程图；图2为仿真的一条飞行航迹；图3为本发明的加入故障检测与隔离(FDI)前后的经度误差的仿真对比图；图4为本发明的加入故障检测与隔离前后的纬度误差的仿真对比图；图5为本发明的加入故障检测与隔离前后的经度误差方差的仿真对比图；图6为本发明的加入故障检测与隔离前后的纬度误差方差的仿真对比图；图7为本发明的末端加入SAR前后的经度误差的仿真对比图；图8为本发明的末端加入SAR前后的纬度误差的仿真对比图；图9为本发明的末端加入SAR前后的经度误差方差的仿真对比图10为本发明的末端加入SAR前后的纬度误差方差的仿真对比图。
具体实施例方式下面结合附图对发明的技术方案进行详细说明如图1所示，本发明的原理是从机载地理系导航的角度入手，建立了地理系下的 位置线性化量测方程，包括GPS/INS量测方程，CNS/气压高度表/INS量测方程和SAR/INS 量测方程。具体实施方法如下一、建立航空机载惯性导航系统的误差状态量方程选择导航坐标系为东北天地理水平坐标系(OnXnYnZn)，采用线性卡尔曼滤波器进 行组合，系统的状态方程为惯性导航系统的误差状态量方程，通过对惯性导航系统的性能 及误差源的分析，可以获得惯导系统的误差状态量方程为 式中义
其中ΦΕ，ΦΝ，
小！为平台误差角；SvE，δνΝ, δ Vu为速度误差；δ L，δ λ , Sh为纬度、经度和高度误差； ebx，eby, ebz, ε rx, ε ry, ε 分别为陀螺常值漂移误差和一阶马尔可夫漂移误差？H 为加速度计零偏。二、建立航空机载地理系下位置观测量的线性化量测方程1. GPS/INS 量测方程 式(1)中，VpVyVyVp v5、V6分别为GPS输出的北向、东向、高度方向位置误差和 东向、北向、天向速度误差，均考虑为白噪声。2. CNS/气压高度表/INS量测方程 式(2)中v7、v8, V9分别为CNS和气压高度表输出的北向、东向、高度方向位置误 差，均考虑为白噪声。3. SAR/INS 量测方程 式(3)中v1Q为SAR图像匹配输出时的航向角度误差，V11为北向位置误差，V12为 东向位置误差，其大小依赖于所采用的图像匹配定位算法的精度。误差均考虑为白噪声。三、故障检测与隔离算法，判断导航传感器是否有故障4.故障检测算法的实现在容错组合导航系统中，必须实时地确定各子系统滤波器处理的量测信息的有效 性，这就要求在子滤波器的设计中，应配备实时的故障检测和隔离算法，针对本文提出的组 合导航系统的特点(此系统采用闭环校正)，子滤波器采用残差χ2检验法进行故障检测和 隔罔。计算故障检测函数λ (k) 式中=Hi为第i个子系统的观测系数矩阵；i,(A:/A:-l)、Pi(Vk-I)为第i个子系 统的最优预测估计值和最优预测估计值误差协方差阵^(k)为第i个子系统的观察噪声
方差阵。λ (k)函数服从自由度为m的χ 2分布，m为量测矩阵Z的维数。故障判断的准则 为 式中，Td为预先设定的门限，由预警率Pfa预先确定。若判断出子系统无故障，则把 其滤波值送至主滤波器；若检测出子系统有故障，则对其进行隔离，并通过系统重构使整体 系统不致因故障而失效，当检测到子系统恢复正常后，再重新将其滤波值送入联邦滤波器。四、进行KF(Kalman Filter)滤波，估计航空机载惯性导航系统的误差状态量5.状态方程和量测方程的离散化及卡尔曼滤波器当采用线性卡尔曼滤波器时，需要对上面连续形式的系统状态方程(6)和量测方 程(7)、(8)、(9)进行离散化，从而获得离散形式的系统方程。其离散化形式如下
剛

式中'
(项肝广如⑷丁，T 为迭代周期。 从而可以获得系统的线性化卡尔曼滤波器方程如下 上式中的Q，R分别为系统的噪声方差矩阵和量测方差矩阵。6.联邦主滤波器信息融合方程和融合算法 式中η = 1，2，3为该时刻起作用的子滤波器的个数，由故障检测算法决定。图2 图6的仿真结果表明，系统没有加入故障检测单元(FDI)时，系统的恢复速 度慢，定位精度受GPS故障影响较大，系统误差协方差阵不能够正确的跟踪误差的实际变 化；加入FDI后，系统的恢复速度变快，定位精度由INS/CNS决定，定位精度大大提高，系统 误差协方差阵也能够正确跟踪误差的实际变化。图7 图10的仿真结果表明，系统在末端接入SAR辅助系统后，系统的经度和纬 度误差比未接入SAR时小很多，末端定位精度大大提高，误差协方差阵也能够正确跟踪误 差的实际变化。系统的定位精度和稳定性得到显著提高。
权利要求
一种高空长航无人机的多传感器容错自主导航方法，其特征在于包括以下步骤(1)通过建立航空机载惯性导航系统INS的误差状态量方程，得到对航空机载惯性导航系统误差状态量的数学描述，航空机载惯性导航系统INS误差状态量X为φE，φN，φU分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δvE，δvN，δvU分别表示航空机载惯性导航系统误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL，δλ，δh分别表示航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；εbx，εby，εbz，εrx，εry，εrz分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，T为转置；(2)采用航空机载地理系下位置线性化观测方法，建立航空机载地理系下位置观测量与被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量之间的线性化量测方程，包括GPS/INS量测方程，CNS/气压高度表/INS量测方程和SAR/INS量测方程；(3)子系统采用残差χ2检验法进行故障检测和隔离；(4)将步骤(2)所述的纬度、经度和高度误差状态量进行卡尔曼滤波，同时按步骤(3)对子系统观测量和状态量进行故障检测，当子系统无故障时，将子系统卡尔曼滤波结果送入联邦滤波器；当子系统有故障时，子系统将被隔离，卡尔曼滤波结果也不能送入联邦滤波器，其他无故障子系统构成新的联邦滤波系统；(5)联邦滤波器对子系统送来的数据进行数据融合，输出全局最优估计值，从而对航空机载惯性导航系统的导航误差进行修正。FSA00000136860600011.tif,FSA00000136860600012.tif
2.根据权利要求1所述的高空长航无人机的多传感器容错自主导航方法，其特征在 于步骤(2)中所述的航空机载地理系下位置观测量和被估计的步骤(1)所述的航空机载 惯性导航系统误差状态量中的经度、纬度和高度误差状态量之间的线性化量测方程如下DGPS/INS量测方程 (1)式(1)中，VpVyVpVpVpVe分别为GPS输出的北向、东向、高度方向位置误差和东向、 北向、天向速度误差，均考虑为白噪声； 2) CNS/气压高度表/INS量测方程 式(2)中v7、v8、v9分别为CNS和气压高度表输出的北向、东向、高度方向位置误差，均 式⑶中v1(1为SAR图像匹配输出时的航向角度误差，vn为北向位置误差，v12为东向 位置误差，误差均考虑为白噪声。
3.根据权利要求1所述的高空长航无人机的多传感器容错自主导航方法，其特征在 于步骤(4)中所述的故障检测方法如下 计算故障检测函数\ (k) 式中母为第i个子系统的观测系数矩阵；戈、Pi(k/k-l)为第i个子系统的最 优预测估计值和最优预测估计值误差协方差阵氓00为第i个子系统的观察噪声方差阵； A (k)函数服从自由度为m的残差x 2分布，m为量测矩阵Z的维数；故障判断的准则为 j式中，TD为预先设定的门限，由预警率Pfa预先确定；若判断出子系统无故障，则把其滤 波值送至主滤波器；若检测出子系统有故障，则对故障子系统进行隔离，并通过系统重构使 整体系统不致因故障而失效，当检测到子系统恢复正常后，再重新将其滤波值送入联邦滤 波器。(5)
全文摘要
本发明公布了一种可用于高空长航无人机的多传感器容错自主导航方法，该方法首先对GPS、天文(CNS)和SAR(合成孔径雷达)三种导航传感器的工作环境和工作特性进行了理论分析，结合惯性/GPS，惯性/天文，惯性/SAR在航空机载地理系下的位置组合观测原理，建立了地理系下位置观测线性化量测方程；然后分析了导航传感器的故障特点，并模拟GPS故障时的输出，建立了相应的故障检测算法单元，对子滤波器进行故障检测与隔离；最后设计完成了基于天文、SAR辅助的惯性/GPS组合导航系统数学模型，采用联邦滤波方法对惯性导航的误差状态进行了最优估计。本发明导航精度高，能充分发挥地理系下多传感器组合导航对航空机载惯性导航系统误差状态量的估计作用。
文档编号G01C21/24GK101858748SQ20101018753
公开日2010年10月13日 申请日期2010年5月28日 优先权日2010年5月28日
发明者于永军, 刘建业, 张丹, 熊智, 王丹, 王融, 郁丰, 陈海明, 陈计辉 申请人:南京航空航天大学