亚星游戏官网-www.yaxin868.com

专利名称：一种适用于微惯性与全球定位组合导航系统的分布式滤波装置的制作方法
技术领域：
本发明涉及一种滤波装置，更特别地说，是指一种适用于微惯性与全球定位组合导航系统(MIMU/GPS)的分布式滤波装置。

背景技术：
微惯性测量系统(MIMU)是由微机械陀螺、微机械加速度计和集成电路组成的惯性导航系统，具有完全自主性、不受外界干扰、短期精度高及数据更新快的优点，且成本低、尺寸小、重量轻、功耗低。不足之处在于精度低、系统误差随时间积累，难以胜任长时间导航任务。
全球定位系统(GPS)是一种卫星导航系统，具有定位精度高、长期稳定性的优点。不足之处在于自主性差、信号易失锁、采样率偏低，无法应用在高动态、高干扰环境中。
MIMU/GPS组合导航系统将MIMU与GPS组合，利用GPS的长期稳定性和高精度弥补MIMU误差随时间积累的缺点，利用MIMU的短期高精度弥补GPS系统易受干扰、信号易失锁的缺点。其实质是将MIMU的速度误差、姿态误差、位置误差和器件误差作为状态量，将GPS提供的速度作为观测量，通过信息融合技术估计出MIMU的各种误差，然后进行反馈校正。
在MIMU/GPS组合导航系统中，常采用的信息融合技术为卡尔曼滤波。卡尔曼滤波技术分为集中式(centralized)和分散式(decentralized)两类。集中式卡尔曼滤波将各子系统的观测数据送至信息融合中心进行集中处理，理论上讲，该方法可以得到严格的最优状态估计，缺点是对处理器的要求高、数据量大、实时计算和控制困难、可靠性较低、容错性差；当滤波维数较大时，在实际应用中难以实现。在分散式卡尔曼滤波中，各局部滤波器利用相应子系统的观测值，得到局部状态最优估计，而后备局部估计送至融合中心，融合中心将局部估计融合成全局估计。分散式卡尔曼滤波由于采用了多处理器并行处理的分层结构方式，因此计算量小、可靠性高、容错性好，也便于实现系统多层次故障检测与诊断。通过信息分配原理，将整个系统中的状态信息分配到多个局部卡尔曼滤波器中，通过局部滤波器和主滤波器的逐步滤波，形成了联邦卡尔曼滤波。联邦卡尔曼滤波使得系统在降低运算量和提高容错能力的同时，滤波器的设计更为灵活，同时联邦卡尔曼滤波的滤波结果在理论上和集中式卡尔曼滤波等价。然而联邦卡尔曼滤波中信息分配因子的确定是一个难点，如果分配不当，则会影响滤波结果。
前述滤波器的设计往往是基于一个参数固定的系统模型，但由于运动载体机动、外部扰动以及环境条件的变化，系统模型参数会随之变化，这时具有固定模型的常规滤波器难以跟随参数的实际变化，使得滤波性能下降，针对此问题出现了多模型(Multiple Model，MM)估计器。MM方法选择或设计一个模型集来描述系统可能的行为模式，系统的总体估计是多个并行运行的卡尔曼滤波器估计的联合，其中每个滤波器与特定的系统模式相匹配。在现有实际可行的MM方法中，交互式多模型(Interactive Multiple Model，IMM)方法被认为是迄今为止最有效的多模型方法之一，其性能和计算量比达到了最优。但是MM方法要求多个模型并行运行，计算量较大，在低成本组合导航系统中，受硬件处理能力的限制，使得MM方法的应用受阻。
另外，微机械陀螺和微机械加速度计的精度较低，用高阶卡尔曼滤波器进行较精确的建模会产生较大的模型误差，同时，模型阶数的增加导致计算量呈三次方上升，限制了工程应用。


发明内容
本发明针对低成本MIMU/GPS组合导航系统提出了一种分布式滤波的装置，该滤波装置能够减小系统的计算量，以及高容错能力，同时通过实时估计陀螺漂移并进行补偿，提高了MIMU/GPS组合导航系统输出信息的精度。
本发明的一种适用于微惯性与全球定位组合导航系统的分布式滤波装置，MIMU进行惯性导航解算后得到速度、姿态和位置信息，GPS提供航向和速度信息；该分布式滤波装置中包含5个差值比较单元和5个滤波器；所述的5个差值比较单元是指A差值比较单元(11)、B差值比较单元(21)、C差值比较单元(31)、D差值比较单元(41)和E差值比较单元(51)；所述的5个滤波器是指一阶东向速度滤波器(10)、一阶北向速度滤波器(20)、七阶姿态卡尔曼滤波器(30)、一阶东向位置滤波器(40)和一阶北向位置滤波器(50)；该分布式滤波装置利用MIMU和GPS的航向、速度信息作为量测输入进行滤波，滤波所得到的信息反馈到MIMU中进行校正，最后经过校正后的速度、姿态和位置信息作为MIMU/GPS组合导航系统的输出。
本发明分布式滤波装置的优点为 1、对采用低成本微惯性器件的MIMU/GPS组合导航系统应用分布式滤波方式进行信息处理，由于滤波器维数降低，减小了系统的计算量，增强了实时性。
2、采用分布式滤波能够弥补MIMU误差随时间积累的缺点，增强MIMU与GPS进行组合系统鲁棒性和稳定性。
3、采用分布式滤波能够弥补MIMU/GPS组合导航系统中在GPS信号受干扰时能起到良好的估计效果，增强了适应性； 4、在分布滤波过程中，对MIMU和GPS输出的速度和位置信息进行融合，有效地增强了滤波容错性，补偿了陀螺漂移，提高了导航精度。



图1为MIMU/GPS组合导航系统的结构简示图。
图2为本发明分布式滤波器的滤波原理图。

具体实施例方式 参见图1所示，MIMU/GPS组合导航系统包括有MIMU、GPS和分布式滤波器。MIMU进行惯性导航解算后得到速度、姿态和位置信息。GPS提供航向和速度信息。分布式滤波装置利用MIMU和GPS的航向、速度信息作为量测输入进行滤波，滤波所得到的信息反馈到MIMU中进行校正，最后经过校正后的速度、姿态和位置信息作为MIMU/GPS组合导航系统的最终输出。
参见图2所示，本发明设计的分布式滤波装置，该滤波装置中包含5个差值比较单元和5个滤波器。
所述的5个差值比较单元是指A差值比较单元11、B差值比较单元21、C差值比较单元31、D差值比较单元41和E差值比较单元51。
所述的5个滤波器是指一阶东向速度滤波器10、一阶北向速度滤波器20、七阶姿态卡尔曼滤波器30、一阶东向位置滤波器40和一阶北向位置滤波器50。
(一)东向速度误差 A差值比较单元11用于接收微惯性测量系统输出的东向速度VEINS和全球定位系统输出的东向速度VEGPS，并将VEINS与VEGPS的差值δVE输出给一阶东向速度滤波器10； 一阶东向速度滤波器10一方面对接收的δVE依据预测/滤波关系进行预测或者滤波判断，若满足预测/滤波关系则进行预测处理；若不满足预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F10处理后输出东向速度误差估计值

PDOP表示位置精度强弱度，SV表示GPS可见星的数目。
在本发明中，将δVE作为观测量即可搭建东向速度滤波器，滤波方程为式中

表示当前时刻k的状态估计值；

表示前一时刻k-1的状态估计值；Pk表示当前时刻k的误差协方差矩阵；Pk-1表示前一时刻k-1的协方差矩阵；Q表示MIMU/GPS组合导航系统下的噪声矩阵；R表示量测噪声矩阵；Zk表示当前时刻k的量测变量。
本发明一阶东向速度滤波器10的初始化参数如下状态变量X0＝0，误差协方差矩阵P0＝100。
在预测模式下，一阶东向速度滤波器10输出的东向速度误差预测值

受噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则东向速度滤波器的滤波方程为 (二)北向速度误差 B差值比较单元21用于接收微惯性测量系统输出的北向速度VNINS和全球定位系统输出的北向速度VNGPS，并将VNINS与VNGPS的差值δVN输出给一阶北向速度滤波器20； 一阶北向速度滤波器20一方面对接收的δVN依据预测/滤波关系进行预测或者滤波判断，若满足预测/滤波关系则进行预测处理；若不满足预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F20处理后输出北向速度误差估计值

PDOP表示位置精度强弱度，SV表示GPS可见星的数目。
在本发明中，将δVN作为观测量即可搭建北向速度滤波器，滤波方程为式中

表示当前时刻k的状态估计值；

表示前一时刻k-1的状态估计值；Pk表示当前时刻k的误差协方差矩阵；Pk-1表示前一时刻k-1的协方差矩阵；Q表示MIMU/GPS组合导航系统下的噪声矩阵；R表示量测噪声矩阵；Zk表示当前时刻k的量测变量。
本发明的北向速度滤波器初始化参数如下状态变量X0＝0，误差协方差矩阵P0＝100。
在预测模式下，一阶北向速度滤波器20输出的北向速度误差预测值

受噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则北向速度滤波器的滤波方程为 (三)姿态误差角、陀螺漂移量 C差值比较单元31用于接收微惯性测量系统输出的东向速度VEINS和北向速度VNINS，以及全球定位系统输出的东向速度VEGPS和北向速度VNGPS，并将VEINS与VEGPS的差值δVE和VNINS与VNGPS的差值δVN输出给七阶姿态滤波器30； 在本发明中，七阶姿态卡尔曼滤波器30中的状态变量量测变量则状态方程量测方程其中 和 ΦE表示东向误差角；ΦN表示北向误差角；ΦUP表示方位失准角；εx表示X轴陀螺漂移量；εy表示Y轴陀螺漂移量；Re表示地球半径；g表示重力加速度；fE表示东向等效比力；fN表示北向等效比力；τr表示陀螺漂移马尔科夫过程的时间常数；w1表示加表引起的输入噪声的东向分量；w2表示加表引起的输入噪声的北向分量；w3表示X轴陀螺的输入噪声；w4表示Y轴陀螺的输入噪声；w5表示Z轴陀螺的输入噪声；w6表示X轴陀螺的马尔科夫噪声；w7表示Y轴陀螺的马尔科夫噪声；vE表示东向速度误差噪声；vN表示北向速度误差噪声。
七阶姿态卡尔曼滤波器30对接收的δVE和δVN进行卡尔曼滤波方法处理后输出姿态误差估计值和陀螺漂移估计值

表示东向误差角估计值；

表示北向误差角估计值；

表示方位失准角估计值；

表示X轴陀螺漂移估计值；

表示Y轴陀螺漂移估计值。
本发明的七阶姿态卡尔曼滤波器30初始化参数如下状态变量误差 协方差矩阵 (四)东向位置误差 D差值比较单元41用于接收微惯性测量系统输出的东向位置EINS和全球定位系统输出的东向位置EGPS，并将EINS与EGPS的差值δE(δE＝EINS-EGPS)输出给一阶东向位置滤波器40； 一阶东向位置滤波器40一方面对接收的δE依据位置预测/滤波关系V40＝{PDOP＞7 or SV＜7}进行预测或者滤波判断，若满足位置预测/滤波关系则进行预测处理；若不满足位置预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F40处理后输出东向位置误差估计值

PDOP表示位置精度强弱度，SV表示GPS可见星的数目。
在本发明中，将δE作为观测量即可搭建东向位置滤波器，滤波方程为式中

表示当前时刻k的状态估计值；

表示前一时刻k-1的状态估计值；Pk表示当前时刻k的误差协方差矩阵；Pk-1表示前一时刻k-1的协方差矩阵；Q表示MIMU/GPS组合导航系统下的噪声矩阵；R表示量测噪声矩阵；Zk表示当前时刻k的量测变量。
本发明的东向位置滤波器初始化参数如下状态变量X0＝0，误差协方差矩阵P0＝10000。
在预测模式下，一阶东向位置滤波器40输出的东向位置误差预测值

受噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则东向位置滤波器的滤波方程为 (五)北向位置误差 E差值比较单元51用于接收微惯性测量系统输出的北向位置NINS和全球定位系统输出的北向位置NGPS，并将NINS与NGPS的差值δN(δN＝NINS-NGPS)输出给一阶北向位置滤波器50； 一阶北向位置滤波器50一方面对接收的δN依据位置预测/滤波关系V50＝{PDOP＞7 or SV＜7}进行预测或者滤波判断，若满足位置预测/滤波关系则进行预测处理；若不满足位置预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F50处理后输出北向位置误差估计值

PDOP表示位置精度强弱度，SV表示GPS可见星的数目。
在本发明中，将δN作为观测量即可搭建北向位置滤波器，滤波方程为式中

表示当前时刻k的状态估计值；

表示前一时刻k-1的状态估计值；Pk表示当前时刻k的误差协方差矩阵；Pk-1表示前一时刻k-1的协方差矩阵；Q表示MIMU/GPS组合导航系统下的噪声矩阵；R表示量测噪声矩阵；Zk表示当前时刻k的量测变量。
本发明的北向位置滤波器初始化参数如下状态变量X0＝0，误差协方差矩阵P0＝10000。
在预测模式下，一阶北向位置滤波器50输出的北向位置误差预测值

受噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则北向位置滤波器的滤波方程为 采用本发明的分布式滤波方式对某型MIMU/GPS组合导航系统进行输出精度的仿真测试，测试结果表明，经本发明的滤波处理后，MIMU/GPS组合导航系统的输出精度提高了3％～8％，且自适合性强。
仿真条件载体位置10816°，北纬3415°，高度2000m，初始北向速度为90m/s，东向速度和北向速度为零，初始失准角向上0116°，陀螺仪漂移3°/h，陀螺漂移白噪声漂移均方根1°/h，加速度计零偏01001g，速度误差为0.26m/s2。
本发明的一种适用于微惯性与全球定位组合导航系统的分布式滤波装置，该分布式滤波装置分别对MIMU和GPS输出的各个信息首先进行差值比较，然后对差值进行滤波的方式，充分体现了分布滤波带来的计算量的降低、容错能力的提高等优势，保证了MIMU/GPS组合导航系统的导航精度。
权利要求
1.一种适用于微惯性与全球定位组合导航系统的分布式滤波装置，MIMU进行惯性导航解算后得到速度、姿态和位置信息，GPS提供航向和速度信息，其特征在于该分布式滤波装置中包含5个差值比较单元和5个滤波器；
所述的5个差值比较单元是指A差值比较单元(11)、B差值比较单元(21)、C差值比较单元(31)、D差值比较单元(41)和E差值比较单元(51)；
所述的5个滤波器是指一阶东向速度滤波器(10)、一阶北向速度滤波器(20)、七阶姿态卡尔曼滤波器(30)、一阶东向位置滤波器(40)和一阶北向位置滤波器(50)；
该分布式滤波装置利用MIMU和GPS的航向、速度信息作为量测输入进行滤波，滤波所得到的信息反馈到MIMU中进行校正，最后经过校正后的速度、姿态和位置信息作为MIMU/GPS组合导航系统的输出。
2.根据权利要求1所述的适用于微惯性与全球定位组合导航系统的分布式滤波装置，其特征在于A差值比较单元(11)用于接收微惯性测量系统输出的东向速度VEINS和全球定位系统输出的东向速度VEGPS，并将东向速度差值输出给一阶东向速度滤波器(10)；
一阶东向速度滤波器(10)一方面对接收的δVE依据预测/滤波关系进行预测或者滤波判断，若满足预测/滤波关系则进行预测处理；若不满足预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F10处理后输出东向速度误差估计值
PDOP表示位置精度强弱度，SV表示GPS可见星的数目；所述的滤波方程为
表示当前时刻k的状态估计值；
表示前一时刻k-1的状态估计值；Pk表示当前时刻k的误差协方差矩阵；Pk-1表示前一时刻k-1的协方差矩阵；Q表示MIMU/GPS组合导航系统下的噪声矩阵；R表示量测噪声矩阵；Zk表示当前时刻k的量测变量；
在预测模式下，一阶东向速度滤波器(10)输出的东向速度误差预测值
受噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则东向速度滤波器的滤波方程为
3.根据权利要求1所述的适用于微惯性与全球定位组合导航系统的分布式滤波装置，其特征在于B差值比较单元(21)用于接收微惯性测量系统输出的北向速度VNINS和全球定位系统输出的北向速度VNGPS，并将北向速度差值输出给一阶北向速度滤波器(20)；
一阶北向速度滤波器(20)一方面对接收的δVN依据预测/滤波关系进行预测或者滤波判断，若满足预测/滤波关系则进行预测处理；若不满足预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F20处理后输出北向速度误差估计值
PDOP表示位置精度强弱度，SV表示GPS可见星的数目；所述的滤波方程为
表示当前时刻k的状态估计值；
表示前一时刻k-1的状态估计值；Pk表示当前时刻k的误差协方差矩阵；Pk-1表示前一时刻k-1的协方差矩阵；Q表示MIMU/GPS组合导航系统下的噪声矩阵；R表示量测噪声矩阵；Zk表示当前时刻k的量测变量；
在预测模式下，一阶北向速度滤波器(20)输出的北向速度误差预测值
受噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则北向速度滤波器的滤波方程为
4.根据权利要求1所述的适用于微惯性与全球定位组合导航系统的分布式滤波装置，其特征在于C差值比较单元(31)用于接收微惯性测量系统输出的东向速度VEINS和北向速度VNINS，以及全球定位系统输出的东向速度VEGPS和北向速度VNGPS，并将东向速度差值和北向速度差值输出给七阶姿态滤波器(30)；
七阶姿态卡尔曼滤波器(30)中的状态变量量测变量则状态方程量测方程其中
和
ΦE表示东向误差角；ΦN表示北向误差角；ΦUP表示方位失准角；εx表示X轴陀螺漂移量；εy表示Y轴陀螺漂移量；Re表示地球半径；g表示重力加速度；fE表示东向等效比力；fN表示北向等效比力；τr表示陀螺漂移马尔科夫过程的时间常数；w1表示加表引起的输入噪声的东向分量；w2表示加表引起的输入噪声的北向分量；w3表示X轴陀螺的输入噪声；w4表示Y轴陀螺的输入噪声；w5表示Z轴陀螺的输入噪声；w6表示X轴陀螺的马尔科夫噪声；w7表示Y轴陀螺的马尔科夫噪声；vE表示东向速度误差噪声；vN表示北向速度误差噪声；
七阶姿态卡尔曼滤波器(30)对接收的δVE和δVN进行卡尔曼滤波方法处理后输出姿态误差估计值和陀螺漂移估计值
表示东向误差角估计值；
表示北向误差角估计值；
表示方位失准角估计值；
表示X轴陀螺漂移估计值；
表示Y轴陀螺漂移估计值。
5.根据权利要求1所述的适用于微惯性与全球定位组合导航系统的分布式滤波装置，其特征在于D差值比较单元(41)用于接收微惯性测量系统输出的东向位置EINS和全球定位系统输出的东向位置EGPS，并将东向位置差值δE＝EINS-EGPS输出给一阶东向位置滤波器(40)；
一阶东向位置滤波器(40)一方面对接收的δE依据位置预测/滤波关系V40＝{PDOP＞7 or SV＜7}进行预测或者滤波判断，若满足位置预测/滤波关系则进行预测处理；若不满足位置预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F40处理后输出东向位置误差估计值
PDOP表示位置精度强弱度，SV表示GPS可见星的数目；所述的滤波方程
表示当前时刻k的状态估计值；
表示前一时刻k-1的状态估计值；Pk表示当前时刻k的误差协方差矩阵；Pk-1表示前一时刻k-1的协方差矩阵；Q表示MIMU/GPS组合导航系统下的噪声矩阵；R表示量测噪声矩阵；Zk表示当前时刻k的量测变量；
在预测模式下，一阶东向位置滤波器(40)输出的东向位置误差预测值
受噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则东向位置滤波器的滤波方程为
6.根据权利要求1所述的适用于微惯性与全球定位组合导航系统的分布式滤波装置，其特征在于E差值比较单元(51)用于接收微惯性测量系统输出的北向位置NINS和全球定位系统输出的北向位置NGPS，并将北向位置差值δN＝NINS-NGPS输出给一阶北向位置滤波器(50)；
一阶北向位置滤波器(50)一方面对接收的δN依据位置预测/滤波关系V50＝{PDOP＞7 or SV＜7}进行预测或者滤波判断，若满足位置预测/滤波关系则进行预测处理；若不满足位置预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F50处理后输出北向位置误差估计值
PDOP表示位置精度强弱度，SV表示GPS可见星的数目；所述的滤波方程
表示当前时刻k的状态估计值；
表示前一时刻k-1的状态估计值；Pk表示当前时刻k的误差协方差矩阵；Pk-1表示前一时刻k-1的协方差矩阵；Q表示MIMU/GPS组合导航系统下的噪声矩阵；R表示量测噪声矩阵；Zk表示当前时刻k的量测变量；
在预测模式下，一阶北向位置滤波器(50)输出的北向位置误差预测值
受噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则北向位置滤波器的滤波方程为
7.根据权利要求2或3或5或6所述的适用于微惯性与全球定位组合导航系统的分布式滤波装置，其特征在于一阶东向速度滤波器(10)、一阶东向位置滤波器(40)、一阶北向速度滤波器(20)和一阶北向位置滤波器(50)的初始化参数中状态变量X0＝0，误差协方差矩阵P0＝100。
8.根据权利要求4所述的适用于微惯性与全球定位组合导航系统的分布式滤波装置，其特征在于七阶姿态卡尔曼滤波器(30)的初始化参数中状态变量误差协方差矩阵
全文摘要
本发明公开了一种适用于微惯性与全球定位组合导航系统的分布式滤波装置，该滤波装置中包含5个差值比较单元和5个滤波器。所述的差值比较单元和滤波器分别对MIMU和GPS输出的航向、速度信息作为量测输入进行滤波，滤波所得到的信息反馈到MIMU中进行校正，最后经过校正后的速度、姿态和位置信息作为MIMU/GPS组合导航系统的最终输出。本发明滤波装置能够减小系统的计算量，以及高容错能力，同时通过实时估计陀螺漂移并进行补偿，提高了MIMU/GPS组合导航系统输出信息的精度。
文档编号G01C21/20GK101769743SQ20101003360
公开日2010年7月7日 申请日期2010年1月4日 优先权日2010年1月4日
发明者富立, 刘志平, 高鹏, 闫立伟, 张宣, 宋伟 申请人:北京航空航天大学