基于广义卡尔曼滤波的室内定位技术

基于广义卡尔曼滤波的室内定位技术

gps高稳定性免疫复杂模型辅助导航现代战争对武器的伪装要求很高，因此，利用雷达监测雷达设备的非自主导航受到限制，以常规导航和gps卫星导航为代表的自主导航方法得到了广泛应用。对于攻击距离远,飞行时间较长的导弹而言,所采用的导航方法主要有惯导,GPS,以及地形匹配等。匹配法主要用于攻击末段,可以得到较高精度的导航信息,但会受到地形限制,当飞行器在海面或大气层外飞行时就无法使用;GPS是目前最受关注的导航系统,精度可以达到m级,但毕竟需要接收外界的导航信息,在实战中有可能会受到干扰;惯导系统不用接收任何外界信息,完全依靠飞行器内部惯性器件完成导航,具有最强的自主性,然而受到陀螺漂移的影响,使其产生每小时数公里的偏差,无法实现长时间的导航。最理想的导航方法,应该做到完全自主且可以根据外部条件变化进行实时修正。利用地磁场强度进行导航,可以完全依靠弹上设备完成,并且不会出现惯导系统的累积误差。事实上,地磁场早已在用卫星的姿态控制方面,我国的技术人员还成功运用地磁场与重力场挽救了失控的风云一号B星。随着地磁场模型的日趋完善以及微处理器和滤波技术的不断发展和成熟,地磁导航方法于近几年得以迅速发展,将地磁场导航运用在小成本卫星上的工作正在进行中。然而,由于地磁场模型不准确,地磁测量器件精度低等缺点,导致地磁场导航在高精度要求的导弹导航方面的研究很少,尚处于探索阶段。本文在之前研究的基础上,采用地磁场矢量作为观测量,获取导航信息并进行分析。1地磁矢量观测地磁导航,可以使用地磁强度矢量或标量作为观测量。如果使用标量,导航系统比较简单,由于测量信息少,不能保证滤波稳定,当飞行器在等磁线上飞行时无法作出正确估计。为了确保系统的稳定性,需要使用地磁矢量作为观测量。借鉴惯导的原理,将三轴磁强计稳定在一个导航坐标系下,通过测量计算得到东北天三个固定方向上面的地磁场强度,将其作为滤波观测方程,使用广义卡尔曼滤波来估计飞行器的当前位置。对比惯导系统,由于测量到的地磁场强度会随时间位置变化而更新,就可以避免初值和积累误差,而又不失其自主性。1.1航迹系3轴加速度测量这种方法需要的观测值是地磁强度,而地磁场模型是在地心系中建立的,为了把飞行器运动信息与场模型联系,在地心坐标系下建立飞行器运动方程。运动方程变量选取飞行器的地心距R,东经L,余纬λ(纬度的余角),以及飞行器的速度,航迹倾角和航迹偏角,即V,θ,ψV,根据运动关系及选取R,L,λ的意义推导飞行器运动方程。飞行器质心运动方程在航迹系上投影的3个方程式为:{dVdt=aV-gsinθVdθdt=aθ-gcosθ-VcosθdψVdt=aψ(1)⎧⎩⎨⎪⎪⎪⎪⎪⎪dVdt=aV−gsinθVdθdt=aθ−gcosθ−VcosθdψVdt=aψ(1)其中,aV,aθ,aψ分别表示飞行器加速度测量装置在航迹系3轴方向上测得的加速度。分析地面坐标系与地心坐标系的关系,以及地面系与航迹固连系的坐标变换关系可以得到式(2):{˙R=VsinθR˙L=-VcosθsinψVRsinL˙λ=VcosθcosψV(2)⎧⎩⎨⎪⎪⎪⎪R˙=VsinθRL˙=−VcosθsinψVRsinLλ˙=VcosθcosψV(2)整理以上6个方程,取方程(2)中的地心距R=r+h,其中r为参考地球半径,其值取6738km,h为飞行器飞行高度,同时考虑到可能产生的噪声加速度,可得运动模型如下:{˙h=Vsinθ˙L=-VcosθsinψVr+h˙λ=-VcosθcosψV(r+h)sinL˙V-aV-gsinθ+wV˙θ=1V(aθ-gcosθ)+1Vwθ˙ψV=-1Vcosθaψ-1Vcosθwψ(3)⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪h˙=VsinθL˙=−VcosθsinψVr+hλ˙=−VcosθcosψV(r+h)sinLV˙−aV−gsinθ+wVθ˙=1V(aθ−gcosθ)+1Vwθψ˙V=−1Vcosθaψ−1Vcosθwψ(3)令x=[h,L,λ,V,θ,ψV]T,噪声w=[wV,wθ,wψ]T,将u=[aV,aθ,aψ]T看作输入,则运动模型的状态方程形式为:1.2地磁场强度分量使用地磁场强度作为观测方程,首先要获得地磁场模型。高斯对地球磁场的大量测量数据进行研究后,提出用球谐波分析方法来研究,类似于引力位提出地球磁场磁位。地磁场模型可表示为:W(R,L‚λ)=a∞∑n=1n∑m=0[(aR)n+1(gmncosmL+hmnsinmL)Ρmn(cosλ)](4)式中符号的意义如下:W:磁势,矢量,无法直接测量得到;R:地心距;L:东经;λ:余纬,即纬度的余角;a:地球半径;gmn,hmn:高斯系数,由国际地磁参考场(IGRF)给出;Pmn(cosλ)是n次m阶的缔合Legendre多项式。磁势是一个矢量场,并且无法测量,而导航需要得到东北天三个方向的地磁强度值。对W进行分解计算得到天向,北向,东向方向的地磁场强度分量:B=[BR,Bλ,BL]=[∂W∂R1R∂W∂λ-1Rsinλ∂W∂L]B表示地磁场强度矢量,BR,Bλ,BL是在天向,北向,东向方向的分量。代入计算可得:{BR=-∞∑n=1n∑m=0[(n+1)(aR)n+2(gmncosmL+hmnsinmL)Ρmn(cosλ)]Bλ=∞∑n=1n∑m=0[(aR)n+2(gmncosmL+hmnsinmL)⋅dΡmn(cosλdλ]BL=∞∑n=1n∑m=0[(aR)n+2(gmnsinmL-hmncosmL)⋅msinλΡmn(cosλ)](5)以上3式为地磁场强度分量表达式,用来作为滤波估计的观测方程。在实际滤波中,可以根据需要来给出可以满足需求的模型阶数。国际地磁参考场(IGRF)的误差有100多nT(有的地方达200多nT),此外,地磁场的长期变化对于导航的影响是极其微弱的,长期变化较大的中心,变化幅度为180nT/年,相对于导弹的飞行时间,这种变化基本可以忽略。而目前比较好的磁强计精度可以达到10nT。取状态变量:x=[h,L,λ,V,θ,ψV]T,z=[BR,Bλ,BL]T考虑到模型与测量器件噪声,令v=[vR,vλ,vL]T,则观测方程为:z(t)=h(x(t),t)+v(t)2离散系统噪声滤波算法根据之前的分析与推导,滤波系统模型如下:{˙x(t)=f(x(t),t)+g(x(t),t)u(t)+g(x(t),t)w(t)z(t)=h(x(t),t)+v(t)对此系统进行广义卡尔曼滤波,需要计算状态转移矩阵Φk+1,k和测量矩阵Hk:Φk+1,k=eΤF(tk)=Ι+ΤFk+Τ22!F2k+⋯Fk=∂f(x(tk),tk)∂x(tk)Ηk=∂h(x(tk),tk)∂x(tk)离散系统的噪声项统计特性为:E[Wk]=0,E[WkWΤj]=QδΤjE[Vk]=0,E[VkVΤj]=RδΤj根据广义的卡尔曼滤波算法,滤波方程如下:(Xk/k-1=(Xk-1+{f[(Xk-1‚tk-1]+g[(Xk-1‚tk-1]⋅u[tk-1]}Τ(6)(Xk=(Xk/k-1+Κk{Ζk-h[(Xk/k-1,tk]}(7)Kk=Pk/k-1HΤk[HkPk/k-1HΤk+Rk]-1(8)Pk/k-1=Φk/k-1Pk-1ΦΤk,k-1+Qk-1(9)Pk=[I-KkHk]Pk/k-1(10)滤波初始条件为:(X0=E[X0]=mX0,Ρ0=CX03仿真结果分析在之前模型的基础上,使用国际地磁场模型(IGRF)2005所给出的高斯系数,按照巡航导弹巡航段的飞行特性,对导航算法进行仿真验证。假设飞行器在无机动条件下平飞,飞行高度50m,飞行速度为300m/s,初始经度和纬度均为45°,航迹倾角为0,偏角-50°,在各状态均有初始误差的情况下进行滤波,观测方程使用2阶地磁场模型,模型噪声取100nT,磁强计测量噪声取10nT,取加速度噪声各分量的标准差为:[1m/s2,1m/s2,1m/s2],仿真时间为3000s,步长1s,所得估计结果如下:对仿真结果进行分析:①对状态的估计收敛性较好;②滤波达到稳定后,经度估计偏差的标准差为0.03°,纬度估计偏差的标准差为0.04°,按照2δ(95%置信度)换算成实际距离,则经度方向偏差约6km,纬度方向偏差约8km;③高度估计稳定后的偏差小于2000m;该算法采用了地磁场强度矢量作为观测量,参考信息多,算法具有良好的稳定性,但在位置的估计上面误差还有些大。运动方程中的加速度噪声影响一般比较小,误差主要是由观测方程噪声所引起的。由于目前地磁场模型测量精度有限,在此条件下所得的估计精度还不高。随着地磁场模型的精度提高,该算法对位置的估计精度可以得到大幅提高。在只考虑磁强计的噪声的条件下进行仿真,则高度估计误差降为300m,经纬度的估计偏差可以达到0.01°左右,换算成实际距离,则经度和纬度方向的估计偏差约为1km～1.5km。4基于国际地磁场模型的滤波将地磁场强度作为矢量考虑,通过测量东北天三个方向上面地磁强度作为观测量来进行滤波。由于观测值有3个,参考信息多,因此算法的稳定性比较好,不要求初值准确,能够克服噪声的突变。然而这样需要平台将磁强计稳定在导航坐标系中,实现起来比较复杂。为了简化系统,可以考虑使用地磁强度标量来滤波,使用测量到的三个垂直方向的磁场强度计算得到当地的地磁强度大小作为观测值,但这种方法稳定性不好,当飞行器在等磁线上飞行时无法做出正确估计,必须使用其他参考信息来辅助导航。为了体现一般性,使研究的导航方法适用与全球各处,本文使用国际地磁