一种多传感器组合导航实时仿真系统的研究

一种多传感器组合导航实时仿真系统的研究

为了解决多传感器信息的融合问题，speyer和kerr在1979年至1987年间提出了并行分散滤波。在1988年的情况下，kalason提出了联邦滤波。与第一种相比，后者信息传输量小，滤波精度高，容错性强，成为美国的新一代通用滤波。尽管联邦滤波的实际应用考虑了不同的通用软件，但该方法对更新频率有特殊要求。也就是说，在融合期间，传输的子传感器的输出是同步的，这在实际系统中通常是不可能的。在这种情况下，许多中国科学家对联邦滤波进行了改进，并取得了良好的结果。但在实践工程中,联邦滤波由于多个滤波器的同时存在,大大降低了实时性,这在高动态环境下显得尤为不利.基于这一点,本文对实时性好的集中滤波加以改进,解决了多传感器异步组合的问题,并在此基础上提出量测滞后的方案.实时仿真系统的验证表明该算法融合了2种滤波的优点,具有较高的滤波精度.1gps-cns-sins组合导航数学模型本文以输出速度、位置信息的GPS、输出三维姿态信息的捷联天文导航系统CNS和捷联惯导系统SINS为组合对象进行多传感器组合研究.1.1系统状态变量的定义采用SINS的误差方程作为系统的状态方程,导航坐标系选用北东地坐标系,通过对SINS系统的性能及误差源的分析,可以获得滤波器的误差方程为˙X(t)=F(t)X(t)+G(t)W(t)(1)式中,系统状态变量定义为18阶,即X={ϕΝ,ϕE,ϕD,δvΝ,δvE,δvD,δL,δλ,δh,εbx,εby,εbz,εrx,εry,εrz,∇x,∇y,∇z}Τ式中,ϕN,ϕE,ϕD为数学平台误差角;δvN,δvE,δvD为速度误差;δL,δλ,δh为纬度、经度和高度误差;εbx,εby,εbz,εrx,εry,εrz分别为陀螺常值漂移误差和一阶马尔可夫漂移误差;∇x,∇y,∇z为加速度计零偏,方程其他参数详见文献.1.2u3000主客观姿态误差组合导航系统共有2组观测值:①GPS给出的速度、位置与捷联惯导系统给出的速度、位置差值,如下式:Ζ1(t)=[vΙΝ-vGΝvΙE-vGEvΙD-vGD(LΙ-LG)RΝ(λΙ-λG)REcosLhΙ-hG]=[δvΝδvEδvDRΝδLREcosLδλδh]+[vΝvEvDΝΝΝEΝD]=ΗGX(t)+VG(t)(2)ΗG=[03×3diag03×303×903×303×3diag[RΝREcosL1]03×9]式中,vN,vE,vD为GPS接收机沿北、东、地方向的速度误差;NN,NE,ND为GPS沿北、东、地方向的位置误差;RN,RE分别为地球子午圈和卯酉圈的曲率半径.②由CNS提供的载体姿态与捷联惯导的姿态计算出的平台误差值,如下式:Ζ2(t)=[ϕΝ+ΟΝϕE+ΟEϕD+ΟD]=[ϕΝϕEϕD]+[ΟΝΟEΟD]=ΗCX(t)+ΟC(3)式中,ΗC=[Ι3×303×15];ON,OE,OD分别为CNS的姿态误差;OC为该误差矩阵.2异步多传感器滤波算法对于上述GPS/CNS/SINS异步多传感器通常采用信息同步处理的联邦滤波.2.1邦滤波器的特点为了确保组合系统具有满意的容错性,可以采用无复位联邦滤波.图1给出其计算流程,虚框内为子滤波器的迭代过程.从图1流程可以看出该模式的联邦滤波具有2个特点:①各子滤波器量测值不同,但状态变量相同;②子滤波器实际上就是集中滤波,包含了2个信息更新过程:时间更新ˆXi(k-1)→ˆXi(k,k-1)Pi(k-1,k-1)→Pi(k,k-1)量测更新ˆXi(k-1)→ˆXi(k‚k)Pi(k-1,k-1)→Pi(k,k)在没有量测值的情况下,时间更新可以保证滤波的连续性.利用这一点,可以使量测信息同步化,从而解决了联邦滤波中异步组合的问题.2.2集中滤波算法借鉴如上特点,集中滤波也可以实现多传感器异步组合.考虑如下的状态方程和量测方程:Xk=Φk,k-1Xk-1+Γk-1Wk-1Ζk=ΗkXk+Vk}(4)可以获得常规的卡尔曼滤波器方程为ˆXk‚k-1=Φk,k-1ˆXk-1(5)ˆXk=ˆXk‚k-1+Κk(Ζk-ΗkˆXk‚k-1)(6)Pk,k-1=Φk,k-1Pk-1ΦTk,k-1+Γk-1Qk-1ΓTk-1(7)Kk=Pk,k-1HTk(HkPk,k-1HTk+Rk)-1(8)Pk=(I-KkHk)Pk,k-1(9)由式(9)可以看出,集中滤波也存在时间更新和量测更新的过程,因此也具备解决异步组合条件.同时借鉴联邦滤波子滤波器的思想,即量测值不同但状态值相同,相应的在集中滤波中设置多个量测矩阵和量测噪声矩阵,根据量测值的不同,用相应的矩阵进行切换,对相同的状态变量进行估计,获得X^k和Pk.图2给出该滤波算法的流程图.在滤波器设计时,选择辅助传感器输出周期的最小值为滤波的固有组合周期,在固有组合时刻没有辅助传感器量测信息时,则进行时间更新;当在某个时刻获得传感器信息时,根据获得的量测值选择H阵、R阵进行滤波,即“谁来谁组合”,从而解决了异步组合的问题.在本文GPS/CNS/SINS组合中,根据传感器周期的不同,一共有3组量测矩阵和量测噪声阵供选择:Η1=ΗGR1=VG}(10a)Η2=ΗCR2=ΟC}(10b)Η3=[ΗC000ΗG0]R3=[ΟC00VG]}(10c)采用这种滤波方式,在任一组合时刻都只有一个滤波器工作,与联邦滤波多个子滤波器同时工作相比,大大提高了系统的实时性;同时通过对X^k的故障检测,可以确认当前传感器是否异常,一旦异常则抛弃当前的滤波值X^k,仅对X^k进行时间更新;从而有效地隔离了故障传感器,并保证了滤波的连续性,具有了联邦滤波容错性强的特点.3基于补偿的量测滞后估计通过上述方法可以很好地解决多传感器异步组合问题.但没有考虑传感器输出滞后的情况.传感器输出滞后在高动态的环境下给量测值Z带来很大的误差,由式(6)可以看出,会导致导航结果恶化,而这种情况单纯依靠滤波本身往往不容易检测.解决量测滞后,文献提出在与滞后量测值对应的过去时刻组合,再将滤波结果递推到当前时刻的方法.这种方法导致在一次滤波中需要进行2次时间更新:组合滤波一次,将滤波结果递推到当前时刻再进行一次,因此算法比较复杂.本文采用补偿的方法,用其他传感器的值补偿滞后量,从而在当前组合时刻进行滤波.如图3所示,在某个组合时刻tk获得带有延时量Δt的量测值Zk,与其对应的时刻是tk-Δt,捷联的解算周期是TINS,用一个数组A以TINS为间隔记录Δt时间内m个主传感器量测值,计算出Zk增量ΔZ,从而将量测值递推到当前时刻,并利用下式:X^k=X^k,k-1+Κk[(Ζk+ΔΖ)-ΗkX^k‚k-1](11)取代图2中对X^k的估计,获得量测滞后修正的最优状态估计.4网络仿真实验假设该组合导航系统的载体是弹道导弹,GPS/CNS/SINS实时仿真系统尽量与实际工程相接近:组合导航程序包括捷联惯导程序、组合滤波程序都用C语言实现,在DOS操作系统下实时运行,采用嵌入式PC104为导航计算机.仿真弹道轨迹如图4(a)所示,包括:垂直上升段OA,拐弯段AB,直线瞄准段BC,被动斜抛段CDE.仿真系统生成的弹道,用Matlab作图如图4(b)所示,发射点120°(E)20°(N)500m;目标点160°(E)60°(N)500m,总射程6092.814km.GPS,CNS和SINS的原始IMU数据都依据弹道仿真生成,整个仿真时间根据弹道计算为656.56s.设GPS的输出周期是1s,带有400ms延时;CNS输出周期是1.5s,不带延时.捷联惯导解算周期是20ms.惯导系统的等效陀螺漂移为0.01(°)/h,等效加速度零偏为10-4g;陀螺一阶马尔可夫过程相关时间为3600s,加速度零偏一阶马尔可夫过程相关时间为1800s.GPS位置误差为10m,速度误差为0.1m/s;CNS姿态误差为20″,设置滤波器工作初始值如表1所示.系统仿真时,根据仿真弹道同步生成的GPS,CNS和SINS原始数据在组合导航计算机内进行实时的组合运算,并输出导航结果.图5给出解决量测滞后的异步传感器集中滤波估计精度和终点误差.从图5可以看出:①虽然滤波器的量测矩阵和量测噪声矩阵是切换的,但滤波曲线是收敛、稳定的,没有因为矩阵的切换出现跳变;②滤波协方差统计表明滤波精度很高,平台误差角维持在10.3″,速度误差为0.25m/s,位置误差为1.5m.由于有无解决量测滞后仅是量测值Z不同,从图2可以看出Z值对P阵的递推没有影响,因此本文采用导航系统终点姿态、速度和位置误差(即组合结果与仿真弹道的差值)表示有无解决量测滞后的区别.表2为某次的仿真结果,其中δR,δP和δY分别表示横滚、俯仰和航向误差.表2组合导航终点误差从表2看出,由于弹道导弹属于高速载体,0.4s的速度延时将造成很大位置误差,通过累计最终导致落点偏差达到公里级.采用量测滞后补偿算法,则落点误差大大减小,优于20m(CEP),同时其他指标精度也获得提高.5基于gps/cns/sins的滤波算法仿真本文对如何在集中滤波下实现异步