技术领域
[0001]
发明涉及一种用于高空长航无人机的多传感器容错自主导航方法，属于航空
飞行器组合导航技术领域，可应用于高空长时间飞行的航空飞行器导航参数的确定，适用于高空长时间飞行的航空飞行器的导航
定位。
背景技术
[0002] 星敏感器作为一种高
精度的天文
姿态敏感器，越来越广泛应用于自主导航领域。星敏感器可以在不需要任何外部基准信息的前提下，直接精确测量获得飞行器相对于惯性
坐标系下的姿态信息，且其测量精度在导航全程保持稳定，现有最高精度可以达到
角秒级。
惯性/天文组合
导航系统以其可以全天候工作，具有极强的自主性和导航误差不随时间发散而在高空、长航飞行器上得到了极大重视和发展，但在
现有技术条件下，由于天文导航定位受到
水平姿态基准的影响，因此，惯性/天文组合导航精度还无法达到惯性/GPS组合导航定位精度。
[0003]
合成孔径雷达(SAR)是二战以后发展起来的一种高
分辨率成像雷达，利用它可以全天候、全天时、远距离的得到类似光学照相的高分辨率雷达图像。用实时SAR图像进行图像匹配定位并和惯导组合构成INS/SAR组合导航系统在近年来得到了发展和广泛重视。但由于SAR成像导航系统在载机应用环境下具有其特殊要求，如SAR成像时一般要求载机处于比较平稳的飞行状态；进行SAR图像匹配时需要耗用不等的匹配计算时间；此外，由于受到机载数字地图
数据库容量的限制，SAR图像导航仅能在部分时段进行工作，目前多用于末端制导。
[0004] 惯性/GPS组合导航系统具有
覆盖范围广、全天候、全天时、高精度和近乎连续实时等特点，用户设备具有体积小、重量轻、功耗小和易于操作等优点。因此惯性/GPS组合导航系统是一种比较理想的导航系统，目前应用最广泛。但是GPS
信号易受
电磁干扰，从而导致INS/GPS定位精度下降；而且GPS隶属于美国，一旦出现战争情况，GPS信号将完全丢失，此时INS/GPS组合导航系统中只有INS发挥作用，误差会不断地积累，影响飞机的导航精度。
[0005] 因此，现有的航空机载惯性/GPS组合的自主导航方法存在可靠性不高和导航精度不稳地性，不能充分满足高空长航无人机对导航精度
稳定性的要求。
发明内容
[0006] 本发明目的在于：克服航空机载惯性/GPS组合导航系统的导航稳定性的不足，提供了一种基于天文、SAR辅助的惯性/GPS多传感器容错自主导航方法即高空长航无人机的多传感器容错自主导航方法。
[0007] 本发明为实现上述目的，采用如下技术方案：
[0008] 本发明基于天文、SAR辅助的惯性/GPS多传感器容错自主导航方法，包括以下步骤：
[0009] (1)通过建立航空机载惯性导航系统INS的误差状态量方程，得到了对航空机载惯性导航系统误差状态量的数学描述，航空机载惯性导航系统误差状态量x定义为：
[0010] φE，φN，φU分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δvE，δvN，δvU分别表示航空机载惯性导航系统误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL，δλ，δh分别表示航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；εbx，εby，εbz，εrx，εry，εrz分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶
马尔可夫漂移误差状态量； 分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴和Z轴方向
加速度计零偏，T为转置；
[0011] (2)采用航空机载地理系下
位置线性化观测原理，建立航空机载地理系下位置观测量和被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的经纬高误差状态量之间的线性化量测方程，包括GPS/INS量测方程，CNS/气压高度表/INS量测方程和SAR/INS量测方程；
[0012] (3)子系统采用残差χ2检验法进行故障检测和隔离；
[0013] (4)将步骤(2)所述的纬度、经度和高度误差状态量进行卡尔曼滤波，同时对子系统观测量和状态量进行故障检测，当子系统无故障时，将子系统卡尔曼滤波结果送入联邦
滤波器；当子系统有故障时，子系统将被隔离，卡尔曼滤波结果也不能送入联邦滤波器，其他无故障子系统构成新的联邦滤波系统。
[0014] (5)联邦滤波器对子系统送来的数据进行数据融合，输出全局最优估计值，从而对航空机载惯性导航系统的导航误差进行修正。
[0015] 本发明与现有技术相比克服了航空机载惯性/GPS组合导航系统的导航稳定性的不足，构建了一种适用于高空长航时无人机的多传感器容错自主导航方法，它具有以下优点：(1)每个局部滤波器互不影响，只是对自己的子系统进行检测。(2)当GPS信息受到干扰时，故障检测单元能够及时检测出故障，从而将GPS子系统隔离开联邦滤波系统；(3)当故障传感器被隔离后，其他局部滤波器仍能继续正常工作，剩余的子系统重新构成新的联邦滤波系统，不影响主滤波器的工作，便于系统重建。
附图说明
[0016] 图1为本发明的多传感器容错自主导航方法的一种
实施例子的
流程图；
[0017] 图2为仿真的一条飞行航迹；
[0018] 图3为本发明的加入故障检测与隔离(FDI)前后的经度误差的仿真对比图；
[0019] 图4为本发明的加入故障检测与隔离前后的纬度误差的仿真对比图；
[0020] 图5为本发明的加入故障检测与隔离前后的经度误差方差的仿真对比图；
[0021] 图6为本发明的加入故障检测与隔离前后的纬度误差方差的仿真对比图；
[0022] 图7为本发明的末端加入SAR前后的经度误差的仿真对比图；
[0023] 图8为本发明的末端加入SAR前后的纬度误差的仿真对比图；
[0024] 图9为本发明的末端加入SAR前后的经度误差方差的仿真对比图；
[0025] 图10为本发明的末端加入SAR前后的纬度误差方差的仿真对比图。
具体实施方式
[0026] 下面结合附图对发明的技术方案进行详细说明：
[0027] 如图1所示，本发明的原理是：从机载地理系导航的角度入手，建立了地理系下的位置线性化量测方程，包括GPS/INS量测方程，CNS/气压高度表/INS量测方程和SAR/INS量测方程。具体实施方法如下：
[0028] 一、建立航空机载惯性导航系统的误差状态量方程
[0029] 选择导航坐标系为东北天地理水平坐标系(OnXnYnZn)，采用线性卡尔曼滤波器进行组合，系统的
状态方程为惯性导航系统的误差状态量方程，通过对惯性导航系统的性能及误差源的分析，可以获得惯导系统的误差状态量方程为：
[0030]
[0031] 式中 其中φE，φN，φU为平台误差角；δvE，δvN，δvU为速度误差；δL，δλ，δh为纬度、经度和高度误差；
εbx，εby，εbz，εrx，εry，εrz分别为陀螺常值漂移误差和一阶马尔可夫漂移误差；
为加速度计零偏。
[0032] 二、建立航空机载地理系下位置观测量的线性化量测方程
[0033] 1.GPS/INS量测方程
[0034]
[0035] 式(1)中，v1、v2、v3、v4、v5、v6分别为GPS输出的北向、东向、高度方向位置误差和东向、北向、天向速度误差，均考虑为白噪声。
[0036] 2.CNS/气压高度表/INS量测方程
[0037]
[0038] 式(2)中v7、v8、v9分别为CNS和气压高度表输出的北向、东向、高度方向位置误差，均考虑为白噪声。
[0039] 3.SAR/INS量测方程
[0040]
[0041] 式(3)中v10为SAR图像匹配输出时的航向角度误差，v11为北向位置误差，v12为东向位置误差，其大小依赖于所采用的图像匹配定位
算法的精度。误差均考虑为白噪声。
[0042] 三、故障检测与隔离算法，判断导航传感器是否有故障
[0043] 4.故障检测算法的实现
[0044] 在容错组合导航系统中，必须实时地确定各子系统滤波器处理的量测信息的有效性，这就要求在子滤波器的设计中，应配备实时的故障检测和隔离算法，针对本文提出的组2
合导航系统的特点(此系统采用闭环校正)，子滤波器采用残差χ 检验法进行故障检测和隔离。
[0045] 计算故障检测函数λ(k)：
[0046]
[0047] 式中：Hi为第i个子系统的观测系数矩阵； Pi(k/k-1)为第i个子系统的最优预测估计值和最优预测估计值误差协方差阵；Ri(k)为第i个子系统的观察噪声方差阵。
[0048] λ(k)函数服从
自由度为m的χ2分布，m为量测矩阵Z的维数。故障判断的准则为
[0049]
[0050] 式中，TD为预先设定的
门限，由预警率Pfa预先确定。若判断出子系统无故障，则把其滤波值送至主滤波器；若检测出子系统有故障，则对其进行隔离，并通过系统重构使整体系统不致因故障而失效，当检测到子系统恢复正常后，再重新将其滤波值送入联邦滤波器。
[0051] 四、进行KF(Kalman Filter)滤波，估计航空机载惯性导航系统的误差状态量[0052] 5.状态方程和量测方程的离散化及卡尔曼滤波器
[0053] 当采用线性卡尔曼滤波器时，需要对上面连续形式的系统状态方程(6)和量测方程(7)、(8)、(9)进行离散化，从而获得离散形式的系统方程。其离散化形式如下：
[0054]
[0056] 从而可以获得系统的线性化卡尔曼滤波器方程如下：
[0057]
[0058]
[0059] Pk|k-1＝Φk，k-1Pk-1Φk，k-1T+Γk-1Qk-1Γk-1T
[0060]
[0061]
[0062] 上式中的Q，R分别为系统的噪声方差矩阵和量测方差矩阵。
[0063] 6.联邦主滤波器信息融合方程和融合算法
[0064]
[0065]
[0066] 式中n＝1，2，3为该时刻起作用的子滤波器的个数，由故障检测算法决定。
[0067] 图2～图6的仿真结果表明，系统没有加入故障检测单元(FDI)时，系统的恢复速度慢，定位精度受GPS故障影响较大，系统误差协方差阵不能够正确的
跟踪误差的实际变化；加入FDI后，系统的恢复速度变快，定位精度由INS/CNS决定，定位精度大大提高，系统误差协方差阵也能够正确跟踪误差的实际变化。
[0068] 图7～图10的仿真结果表明，系统在末端接入SAR辅助系统后，系统的经度和纬度误差比未接入SAR时小很多，末端定位精度大大提高，误差协方差阵也能够正确跟踪误差的实际变化。系统的定位精度和稳定性得到显著提高。
