专利名称：一种微小飞行器自动驾驶用双捷联解算组合导航方法
技术领域：
本发明涉及一种为微小飞行器自动驾驶提供载体运动参数的组合导航系统方法，特别是微小飞行器自动驾驶用双捷联解算组合导航方法。
背景技术：
微小飞行器在地区冲突、局部战争、反恐以及地形测绘等军事和民用领域具有广泛用途，微小飞行器高精度自动驾驶能力是实现上述应用目标的基础，高精度自动驾驶必然需要导航系统提供高精度、实时化载体运动参数，即导航系统不仅需要为航迹规划提供高精度位置、速度信息，更重要的是必须为飞行控制系统提供高平滑度的姿态信息。
微小飞行器在提高性能的同时必须降低成本，这是其在军事和民用领域发挥巨大作用的重要前提。因此，硅MEMS惯性器件成为研制微小飞行器导航系统的首选，但目前硅MEMS惯性器件的精度较低，长时间工作误差较大。满足微小飞行器导航系统一般由硅MEMS惯性器件和非惯性导航传感器共同组成，例如采用磁微传感器提供高精度航向信息，采用压力微传感器提供高精度高度信息。
硅MEMS惯性器件和非惯性传感器构成的多传感器导航系统有两种组成方式第一种方式为分立器件提供导航信息方式，即多个器件独立工作，分别提供不同导航参数给飞行控制系统，这种分立器件提供导航信息方式存在的主要问题是(1)数据精度不一致，部分低精度器件严重制约了整个系统的性能；(2)高精度器件不能通过融合算法估计低精度惯性器件误差并补偿；(3)分立导航器件方式无法解决各个传感器响应时间不一致性问题，从而影响了导航数据的精度和平滑性。第二种方式为基于捷联惯导的组合导航方式，即组合导航方式能够充分利用非惯性测量信息以及捷联解算、组合滤波算法估计低精度惯性器件的误差，并加以补偿，提高IMU测量精度，使系统各个导航参数的精度整体提高。这种基于IMU的组合导航方案解决了分立器件提供导航信息方式的部分问题，但一般的组合导航系统还无法同时兼顾实时性和各个非惯性导航传感器响应延迟的问题。
组合导航系统各个导航传感器响应不同步问题可分为两种情况①各个传感器基本无响应延迟，仅存在采样时刻不同步现象，需要通过插值得到同一时刻的响应后组合滤波；②部分高精度非惯性导航传感器存在响应延迟现象，对响应延迟不做校正直接滤波将导致导航精度和数据平滑度大幅下降；若采用快响应传感器等待慢响应传感器的时间同步方式，必然影响系统的实时性，对微小飞行器控制系统影响较大。
综上所述，组合导航系统第二种响应不同步现象，即高精度非惯性导航传感器响应时间延迟是当前进一步提高系统性能需要解决的核心问题。
发明内容
本发明解决的问题是克服现有技术的不足，提出一种克服组合导航系统部分信息源延迟，且能够为飞行控制系统提供实时化高精度导航信息的微小飞行器自动驾驶用双捷联解算组合导航方法。
本发明的技术解决方案一种微小飞行器自动驾驶用双捷联解算组合导航方法，其特点在于通过下列步骤实现(1)精确标定各个非惯性导航传感器相对IMU数据采集时刻的响应延迟δt；(2)导航计算机实时采集IMU数据，并持续实时进行第一套捷联解算，以该第一套捷联解算结果作为导航系统的输出；(3)在系统中存储大于上述响应延迟δt的最新IMU原始数据，并将非惯性导航传感器输出数据与上述响应延迟δt之前的IMU数据进行组合滤波，通过滤波校正IMU中惯性器件的误差，以上述滤波结果为初始值启动第二套捷联解算；(4)当第二套捷联解算计算到当前时刻，用第二套捷联解算结果取代第一套捷联解算结果，提高第一套捷联解算的精度。
本发明的原理是第一套捷联解算始终为系统实时提供高精度导航信息；同时，间歇性地启动第二套算法，即校正时间延迟后进行组合滤波，估计并校正惯性器件的误差，第二套捷联解算利用高精度滤波结果作为初值，经过快速连续解算，当计算到当前时刻IMU原始输出数据，第二套捷联解算精度必然高于第一套捷联解算，用第二套捷联解算结果取代第一套捷联解算结果，最终提高了输出实用导航信息的第一套捷联解算的精度。
本发明与现有技术相比的优点在于(1)以往的双捷联解算组合导航方案中忽略各个传感器的响应时间差，本发明则首先校正不同传感器的响应延迟；其次，以往双捷联解算方案中包含组合滤波的一套捷联解算持续进行、短时间启动另一套纯捷联解算，即借助组合滤波提高导航精度一路计算持续进行，而仅在某些时刻以该路计算结果为初值，启动一段短时间纯捷联解算。因此，仅在此短时间内得到输出平滑、精度高的导航信息。与之相反，本发明持续进行捷联解算，而根据某些导航传感器延迟响应到来的时刻，短时间启动滤波、校正和另一路捷联解算，并用第二套组合滤波捷联解算的结果在当前时刻取代第一套捷联解算的结果，最终达到在系统全工作周期得到高精度、高平滑度导航数据的目的。
(2)提高了导航系统精度。组合导航的根本目的是利用某些非惯性导航传感器输出高精度导航信息来校正惯性系统的误差。由于载体的运动，若组合导航系统输入数据不是同一时刻的量测信息，必然严重影响组合导航的精度，影响程度取决于各个非惯性导航传感器响应延迟量和载体运动状态变化率，同时还有可能引起飞控数据的跳变。目前一般组合导航算法均未精确测量和校正传感器响应时间的差异，因此影响了导航精度。
(3)能够实时输出高精度导航信息。本发明采用大容量存储器和高效快速计算单元，使得第二套捷联解算在响应延迟的高精度导航传感器下一个数据到来之前已经处理完存储器中积累的全部IMU原始数据，即利用同样的IMU原始数据得到比第一套捷联解算更高精度的导航结果，并在当前时刻替换第一套捷联解算的输出。由于系统确保第一套捷联解算实时进行并输出全部导航参数，因此保证了高精度的导航结果能够实时输出。
(4)本发明中双捷联解算组合导航系统输出数据平滑度好，提高了微小飞行器控制系统的稳定性。在本发明中第一套捷联解算持续进行，实时为控制系统提供全部导航信息，包括姿态和姿态变化率，而不是采用组合滤波和第二套捷联解算的输出送入控制系统，避免了组合滤波引入的高频噪声对控制系统的影响，充分利用了捷联解算数据平滑度好的特点，提高了微小飞行器控制系统的稳定性。
图1为本发明的延迟融合滤波组合导航系统原理示意图；图2为本发明的双捷联解算组合导航方案流程图。
具体实施例方式
为了校正微小飞行器导航系统部分导航传感器响应的延迟，给微小飞行器控制系统提供高精度、实时导航数据，需要完成以下工作(1)精确标定各个非惯性导航传感器相对IMU数据采集时刻的响应延迟δt；将被测试的非惯性导航传感器与IMU共同安装于转台(对于磁传感器应采用非钢表面转台)上，采用自行研制开发的导航器件及系统综合标定补偿控制系统，给测试转台一个阶跃激励，控制系统同时接收IMU和导航传感器的响应时间常数(即达到稳态响应0.707倍时需要的响应时间)，计算非惯性导航传感器与IMU的时间常数差，经过10次以上同样的试验，取时间常数差的均值为响应延迟时间δt。
(2)导航计算机实时采集IMU数据，并持续实时进行第一套捷联解算，以该第一套捷联解算结果作为导航系统的输出。
第一套捷联解算采用惯性导航领域的经典四元数方法。第一套捷联解算从系统启动时就完成初始化并持续利用IMU采样数据实时计算并输出载体的运动参数，其初始化过程由外界一次性的注入。
(3)在系统中存储大于上述响应延迟δt的最新IMU原始数据，并将非惯性导航传感器输出数据与上述响应延迟δt之前的IMU数据进行组合滤波，通过滤波校正IMU中惯性器件的误差，以上述滤波结果为初始值启动第二套捷联解算。
在系统中设计IMU数据存储器，存储当前时刻之前大于δt时间间隔内IMU数据，将响应延迟的高精度非惯性导航传感器数据与δt之前、t0时刻的IMU数据进行组合滤波，并以滤波结果为初值，读取存储器中IMU数据进行第二套捷联解算。
第二套捷联解算同样采用惯性导航领域的经典四元数方法，与第一套捷联解算不同在于，第二套捷联解算在每次滤波完成后启动并采用与第一套捷联解算相同的IMU数据进行导航解算。因此，第二套捷联解算的初始值是经过滤波校正的输出，其精度高于第一套捷联解算当时的结果，因此第二套捷联解算在运行期间精度高于第一套捷联解算，可用于替代第一套捷联解算的导航输出数据。
(4)当第二套捷联解算计算到当前时刻，用第二套捷联解算结果取代第一套捷联解算结果。
为了使t0时刻经过滤波提高精度的导航数据能用于提高当前时刻导航精度，以t0时刻滤波结果为初始值，在实时进行第一套捷联解算的同时启动第二套捷联解算。第二套捷联解算必须连续从存储器中读取数据，补偿惯性器件的误差，并连续快速解算，直至读取当前最新得到的IMU原始数据，此时第一套捷联解算和第二套捷联解算读取相同的当前时刻IMU原始数据，但是第二套捷联解算的结果精度明显会高于第一套捷联解算。此时，采用第二套捷联解算结果取代第一套捷联解算结果，即用第二套捷联解算得到的全部导航信息(三维载体姿态、三维位置、三维速度信息)替换第一套捷联解算的对应导航结果。第一套捷联解算继续采集实时原始数据完成实时捷联解算，为飞控系统提供实时导航信息，同时终止第二套捷联解算。上述工作应该在响应滞后高精度非惯性传感器下一个数据到来之前完成。
当响应滞后的高精度非惯性导航传感器得到第二个数据时，同样与δt时间之前t1时刻的第一套捷联解算结果进行组合滤波，并以滤波结果一方面反馈校正惯性器件的误差，一方面作为初始条件，连续读取存储器中t1时刻起始的IMU数据，并连续快速计算，直至读取当前最近得到的IMU原始数据，第二套捷联解算的结果取代第一套捷联解算结果，第一套捷联解算继续进行，第二套捷联结算终止。上述工作应该在响应滞后的传感器第三个数据到来之前完成。上述工作如此循环往复的进行。
这样第一套捷联解算始终为系统实时提供高精度的导航信息，校正响应时间延迟后的组合滤波能够给出惯性器件的误差，通过补偿IMU原始数据中惯性器件的误差，同时提高了第一套和第二套捷联解算结果的精度；第二套捷联解算还利用高精度的滤波结果作为初值，经过快速连续解算，并取代第一套捷联解算的结果，最终提高了给微小飞行器控制系统提供实用导航信息的第一套捷联解算的精度。
图1中上一排圆圈表示IMU数据接收到的时刻，IMU数据输出频率高且实时性好；下一排圆圈表示其他非惯性导航传感器接收数据时刻，其特点是输出频率低、精度高、存在延迟。非惯性导航传感器接收数据后与δt时刻之前的IMU数据完成组合滤波，并在滤波之后启动第二套捷联解算，经过连续快速的捷联解算，当采用当前最新IMU数据完成第二套捷联解算后，采用第二套捷联解算结果替代第一套捷联解算的对应导航结果。
图2为整个校正非惯性导航器件响应延迟的组合导航系统工作流程图。在第一个判断结束之后无论是、否都需要返回读取IMU数据，保证第一套捷联解算的实时持续进行。第二套捷联解算必须在下一个非惯性导航传感器数据到来之前，处理完所有积累的导航数据。
权利要求
1.一种微小飞行器自动驾驶用双捷联解算组合导航方法，其特征在于通过下列步骤实现(1)精确标定各个非惯性导航传感器相对IMU数据采集时刻的响应延迟δt；(2)导航计算机实时采集IMU数据，并持续实时进行第一套捷联解算，以该第一套捷联解算结果作为导航系统的输出；(3)在系统中存储大于上述响应延迟δt的最新IMU原始数据，并将非惯性导航传感器输出数据与上述响应延迟δt之前的IMU数据进行组合滤波，通过滤波校正IMU中惯性器件的误差，以上述滤波结果为初始值启动第二套捷联解算；(4)当第二套捷联解算计算到当前时刻，用第二套捷联解算结果取代第一套捷联解算结果，提高第一套捷联解算的精度。
2.根据权利要求1所述的微小飞行器自动驾驶用双捷联解算组合导航方法，其特征在于所述的精确标定各个非惯性导航传感器相对IMU的响应延迟δt的方法步骤为(1)输入测试阶跃激励，测量IMU和响应延迟非惯性导航导航传感器各自的响应时间常数；(2)计算响应延迟导航传感器与IMU的时间常数差，经过多次试验取均值得到响应延迟δt。
3.根据权利要求1所述的微小飞行器自动驾驶用双捷联解算组合导航方法，其特征在于所述的步骤(4)中第二套捷联解算连续读取并计算所存储的IMU数据，组合滤波和第二套捷联解算必须在下一个响应延迟的非惯性导航传感器数据到来之前全部完成IMU原始数据的处理，将当前时刻计算结果送入第一套捷联解算后，第二套捷联解算终止。
4.根据权利要求1所述的微小飞行器自动驾驶用双捷联解算组合导航方法，其特征在于在系统中存储大于上述响应延迟δt的最新IMU原始数据的导航计算机存储器，容量应该满足存储大于响应时间δt间隔的最新IMU原始数据，且具有较快的读写速度，以提高第二套捷联解算的效率。
5.根据权利要求1所述的微小飞行器自动驾驶用双捷联解算组合导航方法，其特征在于所述的通过滤波校正IMU中惯性器件的误差，滤波模型中必须包括惯性器件误差作为状态变量，估计出惯性器件误差后在第一套和第二套捷联解算中均做补偿，提高IMU原始数据的精度。
全文摘要
微小飞行器自动驾驶用新型双捷联解算组合导航方案，是一种校正各个非惯性导航传感器响应延迟的组合导航系统，能够实时输出高精度、高平滑度的导航数据。主要内容包括精确标定非惯性导航传感器相对IMU的响应延迟δt；导航计算机实时采集IMU数据并持续实时进行第一套纯捷联解算；在系统中存储大于δt时间长度的最新IMU原始数据；响应延迟的非惯性导航传感器数据与δt时间之前的IMU数据完成组合滤波，通过滤波校正IMU中惯性器件的误差，以滤波结果为初始值启动并连续读取存储器中IMU数据，进行第二套捷联解算；组合滤波和第二套纯捷联解算必须在下一个响应延迟非惯性导航传感器数据到来之前全部完成；当第二套捷联解算计算到当前时刻，用第二套捷联解算结果取代第一套纯捷联解算结果。使第一套纯捷联解算始终实时输出高精度、高平滑度的导航数据。
文档编号G01C21/28GK1811340SQ20061001137
公开日2006年8月2日 申请日期2006年2月24日 优先权日2006年2月24日
发明者盛蔚, 房建成 申请人:北京航空航天大学