专利名称：一种用于光纤陀螺寻北的粒子滤波方法
技术领域：
发明属于多传感器信息融合技术，是一种用于光纤陀螺寻北的数据融合方法，也 适用于自动目标识别与跟踪、自动飞行器导航与控制、机器人、工业过程控制、图像处理、模 式识别等其他多传感器信息融合及应用的领域。
背景技术：
光纤陀螺是基于Sagnac效应的新一代角速率传感器，寻北定向是光纤陀螺的重 要应用之一，在航空、航天和航海等军用领域，以及地球物理探测、大地测量、地下勘探和隧 道挖掘等民用领域都发挥着重要作用。陀螺寻北技术是惯性技术领域的重要组成部分，也 是现代信息化战争中确保武器系统快速、机动、精确打击的重要保障技术之一。随着科学技 术的发展，陀螺寻北仪向着精度高、速度快、体积小、重量轻等方向发展，光纤陀螺的诸多优 点正好迎合了陀螺寻北仪的发展趋势，成为陀螺寻北仪理想的惯性元件。光纤陀螺的寻北精度主要取决于光纤陀螺自身的性能以及所采用的寻北方案。在 光纤陀螺同性能等级的情况下，寻北方案对寻北精度起着决定性的影响。目前光纤陀螺寻 北仪所采用的方法中最为流行的是基于水平动基座的FOG旋转方案，其基本思想是利用光 纤陀螺在两个或多个分布规律位置上对地球自转角速率水平分量的测量输出进行运算，消 除光纤陀螺的常值漂移。粒子滤波作为解决非线性、非高斯动态系统最优估计的最有效的方法，已经成为 研究的热点。本文介绍的基于粒子滤波的光纤陀螺寻北方案，在充分考虑光纤陀螺输出特 性的基础上，建立寻北系统的状态空间模型和观测模型，利用粒子滤波在解决非线性系统 估计问题上的优势，计算出光纤陀螺敏感轴与真北方向的夹角。粒子退化和粒子贫乏、计算量和精度之间的矛盾是粒子滤波应用于光纤陀螺寻北 需要解决的核心问题。粒子贫乏的根源在于重采样是在离散分布上的采样；计算量和精度 之间的矛盾源自蒙特卡洛近似对概率密度函数的低似然区域缺乏描述，必需通过增加粒子 的方法获得对低似然区域的全面描述。解决这两个矛盾的方法可归纳为寻找更优的、更具 效率的粒子，从而获得更优的滤波性能。网格近似方法同蒙特卡洛近似方法类似，它通过一系列带权值的网格来对概率密 度函数进行近似，能够全面地描述概率密度函数在高似然区域和低似然区域的分布情况。 蒙特卡洛近似是通过对概率密度函数进行抽样，以抽样的频度来对概率密度函数进行描 述，因此网格近似相对蒙特卡洛近似具有更高的近似精度.融入网格近似思想的粒子滤波 将有助于提高光纤陀螺寻北的精度。
发明内容
技术问题本发明的目的是提供一种用于光纤陀螺寻北的粒子滤波方法，该方法 可以利用粒子滤波在解决非线性系统估计问题上的优势，计算出光纤陀螺敏感轴与真北方 向的夹角。
5
技术方案本发明为实现上述目的，采用如下技术方案本发明是一种用于光纤陀螺寻北的粒子滤波方法，其特征在于包括下列步骤1)建立单轴光纤陀螺敏感地球自转角速率水平分量的输出特性；2)建立非线性光纤陀螺寻北系统的状态空间模型和观测模型；3)对后验概率密度函数进行混合高斯近似；4)连续滤波密度的网格近似；5)构建基于网格的粒子滤波器；6)最后由导航计算机根据光纤陀螺寻北系统的状态方程和观测方程，采集光纤陀 螺输出信息，并完成寻北的滤波解算。有益效果本发明的方法具有如下优点利用网格近似对滤波密度进行近似，从而获得更具 效率的粒子，提高了粒子滤波的精度，解决了粒子退化和粒子贫乏的问题，并能够达到较好 的寻北精度。对以上发明的有益效果说明如下暂不考虑陀螺的安装误差、转台的水平误差和温度变化等因素。设光纤陀螺常值 漂移为0.2(° )/h，光纤陀螺白噪声为0.2(° )/h，与地理真北方向夹角为60°，地理纬度 为北纬30°，初始状态服从均值为61°，标准差为5°的正态分布。普通粒子滤波和基于网格的粒子滤波精度都随粒子数量增加而提高，为便于比较 两种算法的性能，仿真在保证滤波收敛的情况下选取了较少的粒子数量，两种滤波算法都 抽取粒子数量50个。基于网格的粒子滤波算法以离散形式给出了滤波密度，因此必须将这 一离散的滤波密度转化为连续的滤波密度，仿真中利用高斯近似方法将离散形式的滤波密 度转换为连续的密度函数。对连续滤波密度进行网格近似时，兼顾截断误差和网格密度对 滤波精度的影响，在-1.725 δ 1.725 δ区间内对连续滤波密度均勻划分网格，其中δ为 滤波密度的均方差。寻北时在水平面内相差180°的两个位置上采集陀螺数据，取其差值获 得观测数据。图3所示为基于网格的粒子滤波算法和普通粒子滤波算法的寻北结果的比较，图 4为两种滤波算法用于寻北时的滤波角度误差。仿真时为对比两种滤波算法的差异，在保证 两种滤波算法正常工作的情况下选取了较少的粒子。由图可看出基于网格的粒子滤波算法 寻北角度误差均值在0.07°左右，而普通粒子算法寻北角度误差均值在0.15°左右，基于 网格的粒子滤波算法具有更高的寻北精度。增加粒子数量，两种方法在精度上都将得到进 一步提尚。
图1是网格粒子滤波流程框图。图2是概率密度函数的近似。图3是寻北结果比较。图4是寻北误差比较(绝对值)。
具体实施例方式下面结合附图对发明的技术方案进行详细说明
首先建立单轴光纤陀螺敏感地球自转角速率水平分量的输出特性；然后根据所选 光纤陀螺寻北方案建立非线性光纤陀螺寻北系统的状态空间模型和观测模型；对后验概 率密度函数进行混合高斯近似；对连续滤波密度进行网格近似；构建基于网格的粒子滤波 器；最后由导航计算机根据光纤陀螺寻北系统的状态方程和观测方程，采集光纤陀螺输出 信息，并完成寻北的滤波解算。1)建立单轴光纤陀螺敏感地球自转角速率水平分量的输出特性根据采样和解算方式的不同，光纤陀螺的寻北方案可以分为静态寻北方案和动态 寻北方案，静态寻北方案包括二位置寻北方案、三位置寻北方案和多位置寻北方案等。二位 置的寻北方案具有转动位置少，容易消除陀螺常值漂移，精度高、速度快的特点，因此此处 采用二位置寻北方案。将光纤陀螺固定在转台上，转台相对大地水准面平行，光纤陀螺的输 入轴与转台平面平行。方位测量时，光纤陀螺相对地球静止，则地球自转角速率在光纤陀螺 敏感轴方向上的分量为
ω( ) = coie cosO) cos(6>) + 冲)(1)式中，ω (t)为地球自转角速率在光纤陀螺敏感轴方向上的分量，ω ε为地球自转 角速率…为当地纬度，θ为光纤陀螺敏感轴与北向的夹角，ε (t)为光纤陀螺随机漂移。 ε (t)主要由常值和高斯白噪声构成，表达式为ε (t) = ε 0+G (t)(2)式中，ε ^为光纤陀螺常值漂移，G(t)为高斯白噪声。2)建立非线性光纤陀螺寻北系统的状态空间模型和观测模型寻北时将光纤陀螺固定在水平转台上，此时为位置1，转台水平旋转180°，此时 为位置2，在k时刻将水平面内相差180°的两个位置上的光纤陀螺输出量相减，构成二位 置量测法，能够有效消除光纤陀螺的常值漂移。其二位置量测为
ω\ - ω = cow cos(炉)cos(《) + ε\- a>ie cos(炉)cos(0k -νπ)-ε\ 八、
=2ωκ cos⑷ cos(0k ) + ε\- ε\式中，Ok1为k时刻即第k次滤波在位置1处光纤陀螺的输出，《k2为k时刻在位 置2处光纤陀螺的输出，θ k为光纤陀螺敏感轴与北向的夹角在k时刻的值，￡1/为1^时刻 在位置1处光纤陀螺的随机漂移，ε k2为k时刻在位置2处光纤陀螺的随机漂移。寻北时光纤陀螺敏感轴与北向的夹角θ为一常值，考虑平台振动等的影响，建立 系统的离散状态方程θ k = θ h+Wh(4)式中，θ η为k_l时刻光纤陀螺敏感轴与北向的夹角，Wlri为k-Ι时刻平台振动等 原因引起的系统状态方程随机误差，考虑为高斯白噪声。取二位置量测量的差为观测量，建立系统的量测方程
% =、= cos(代)+ ε「ε\ = cos(^)+v,(5)
2coie cos(炉)2coie cos(p)
7
式中，Zk为k时刻的二位置观测量，Vk为k时刻陀螺漂移引起的观测方程随机误
差，考虑为高斯白噪声。3)对后验概率密度函数进行混合高斯近似要对概率密度函数进行网格划分，则必须得到便于计算的概率密度函数表达式， 而实际情况中粒子滤波通过带权值的粒子点以离散形式给出了滤波密度，因此必须将这一 离散的滤波密度转化为连续的滤波密度。一个简单的处理方法便是高斯近似，其均值与方 差可以通过计算粒子点的均值与协方差获取。 式中，μ k为k时刻滤波密度的均值，< 为k时刻粒子i的权值，Xki为k时刻粒子 i的值，EkSk时刻滤波密度的协方差，N为大于1的自然数。当高斯近似引起较大误差时，可考虑混合高斯模型近似。 式中，ρ(·)为概率密度函数，Ν(·)为高斯分布，χ为状态变量，>0,
为混合高斯模型中第j个高斯分布的权重，称为混合比例，μ j, Σ j, j = 1,2,3,…，N为混合高斯模型中第j个高斯分布的均值和协方差。通过观测数据对混合模型进行拟和的标准方法是EM算法，是一种针对概率模型 设计的迭代优化技术。在混合高斯模型中，EM算法对参数的学习如下给定一组观测值χ = Ix1, X2,…，xM}，混合高斯模型参数由3部分组成 让aij，i = 1，2，3，…，M，j = l，2，3，…N为隐含变量，表示Xi由第j个多维正 态分布产生的概率即~ = p(Ci = j)，C为随机变量，用于指示数据成分，则有E-st印
然后进行M-St印 -Ly 4(13)
J Mti 1式中，上标t和t+Ι表示第t次迭代和第t+Ι次迭代，t为大于1的自然数。4)连续滤波密度的网格近似对任意一个概率密度函数，几种常用的近似方法有高斯近似、网格近似和蒙特卡 洛近似等。图2(a)描述的是蒙特卡洛近似思想，圆圈代表抽样的频度，蒙特卡洛方法采用 统计抽样的方式对概率密度函数进行模拟，抽样越密集表示该区间的概率密度函数越大。 图2(b)描述了网格近似的思想，网格近似假设状态空间是离散并有界的，即状态空间由有 限个离散状态Uk-Aj = 1，2···Ν)组成，该离散状态一般取为网格的中心点，在k-1时刻状 态可以取上述N个状态中的任何一个，对于每个状态xk_/，用WkJ表示每个状态的权值，当 网格均勻分布时，Wk^j取网格中心点的似然值，则k-Ι时刻的概率密度函数可表示为
N
(14)式中，δ ( ·)为脉冲函数，Xk^1为k-Ι时刻状态变量，Z1I1为1时刻到k-Ι时刻观测量。网格近似的方法明显具有更高的精度，在较少格子情况下也能取得较好的精度。 在网格近似中，为了获得对连续状态空间更好的近似，网格必须足够地密集，随着状态空间 维数的增加，也必然导致计算量的剧增。同时，如果状态空间不是有限的，就必须对状态空 间进行截断，由于网格近似中以网格中心点的似然值作为网格权值的近似，存在一定的误 差，因此在网格数量固定的情况下，增大截断的区域，必然使得网格划分更为稀疏，从而增 大网格权值近似的误差。对状态空间进行截断时应遵循的原则是，当概率密度函数分布比 较尖锐时，缩小截断的区域，当概率密度函数分布比较平滑时，增大截断的区域。5)构建基于网格的粒子滤波器在同等数量网格(粒子)情况下，网格近似将比蒙特卡洛近似具有更高的精度，因 此将网格近似的思想融入粒子滤波中，将能取得更好的滤波效果。基于网格的粒子滤波算 法其基本思想在于得到滤波密度后，对其进行高斯或混合高斯近似，将离散的滤波密度转 换为连续的滤波密度，并对该滤波密度进行网格近似而不是蒙特卡洛近似，将获取的具有 权值Wlri的格子作为新的粒子进入下一次滤波循环。由于网格近似相对蒙特卡洛近似具有 更高的效率，且能够更好地描述概率密度函数的低似然区域，因此引入网格近似后，将提高 粒子滤波的精度。同时，由于新粒子是通过连续密度函数获取的，不存在粒子退化和粒子贫 乏的问题。网格粒子滤波权值计算递推过程如下根据Chapman-KoImogrov方程可以得到先验概率密度
(15)
式中，Xk为k时刻状态变量。由于系统在k-Ι时刻的后验概率密度可由(14)式表示，则有 后验概率密度可通过Bayes规则得到
Np(xk I Z议)oc p{zk I Xk、P(Xk I Aw) oc p(zk 丨 Xi)^] W-iK·^ I^1)(口)式中，z1:k为1时刻到k时刻观测量。一般不能从后验概率密度进行采样，利用k-Ι时刻粒子从建议概率密度中采样， 由于k-Ι时刻后验概率密度利用网格进行近似，粒子权值为Wk+因此有
N
式中，q( ·)为建议密度函数，wk_/为k-Ι时刻第i个粒子的权值。 则有 式中，<为未归一化时，第i个粒子在k时刻的权值。则权值计算公式为 归一化权值
式中，Wki为k时刻第i个粒子的权值。 下面给出基于网格的粒子滤波算法基本步骤(如图1所示) Stepl 初始化，在ρ (x0)抽取N个样本点χΛ相应权值1/Ν ； St印2 抽样·< -q(xk I xk.x,Zk) = p(xk I Zlk^)；
St印3 计算权值并归一化 St印4 计算估计值 St印5 对离散的滤波密度进行高斯近似或重采样后利用EM算法进行混合高斯近 似；St印6 对连续的滤波密度在一定的截断区域内进行网格划分；St印7 计算每个网格的权值，取网格中心点似然函数的值为新的权值W ；StepS 将带权值的网格点作为新的粒子X 进行下一周期的滤波；其中，X(1为滤波初值，Xt/为滤波初始粒子，忑为k时刻的滤波估值，WkS W1/为k时 刻粒子的权值和网格近似后的权值，XkSV为k时刻粒子的值和网格中心点的值，i，j = 1， 2,3......N。在给定精度下，基于网格的粒子滤波算法仅需少量粒子，便能达到使用较多粒子 的普通粒子滤波的精度，具有更强的实时性能。最后，基于网格的粒子滤波算法将离散的滤 波密度转化为连续的滤波密度，利用网格生成新的粒子，避免了普通粒子滤波中粒子退化 和粒子贫乏的问题。6)完成光纤陀螺寻北滤波解算由导航计算机根据光纤陀螺寻北系统的状态方程和观测方程，采集光纤陀螺输出 信息，并完成寻北的滤波解算。综上所述，对该方法进行效果分析。基于网格的粒子滤波算法将离散的滤波密度 转化为连续的滤波密度，然后对其进行网格划分，生成新的粒子，避免了普通粒子滤波中粒 子退化和粒子贫乏的问题。在同等条件下将基于网格的粒子滤波算法与粒子滤波算法进行 寻北对比。图2所示为基于网格的粒子滤波算法和普通粒子滤波算法的寻北结果比较，其中 ①为普通粒子滤波器用于寻北的滤波结果；②为Grid-PF粒子滤波器的寻北结果；③为光 纤陀螺敏感轴与北向夹角的真值，由于平台振动等原因围绕60°轻微波动。由图2可以看 出，普通粒子滤波算法和基于网格的粒子滤波算法都可以应用于光纤陀螺的寻北方案中， 并可以得到较为理想的寻北精度。两种滤波算法由初始状态61°迅速收敛至真值60°附 近，且基于网格的粒子滤波算法寻北结果更接近于真实状态60°。图3为两种滤波算法寻北角度的绝对误差，从图3可以看出，基于网格的粒子滤波 算法寻北角度误差均值在0.07°左右，而普通粒子算法寻北角度误差均值在0.15°左右， 可见基于网格的粒子滤波算法，通过引入网格近似的思想，在同等粒子情况下具有比普通 粒子滤波算法更高的精度，将基于网格的粒子滤波算法应用于光纤陀螺的寻北能达到更高 的精度。本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
权利要求
一种用于光纤陀螺寻北的粒子滤波方法，其特征在于包括下列步骤1)建立单轴光纤陀螺敏感地球自转角速率水平分量的输出特性；2)建立非线性光纤陀螺寻北系统的状态空间模型和观测模型；3)对后验概率密度函数进行混合高斯近似；4)连续滤波密度的网格近似；5)构建基于网格的粒子滤波器；6)最后由导航计算机根据光纤陀螺寻北系统的状态方程和观测方程，采集光纤陀螺输出信息，并完成寻北的滤波解算。
2.根据权利要求1所述的一种用于光纤陀螺寻北的粒子滤波方法，其特征在于步骤(1)中所述建立单轴光纤陀螺敏感地球自转角速率水平分量的输出特性的方法如下将光纤陀螺固定在转台上，转台相对大地水准面平行，光纤陀螺的输入轴与转台平面 平行；方位测量时，光纤陀螺相对地球静止，则地球自转角速率在光纤陀螺敏感轴方向上的 分量为 式中，ω (t)为地球自转角速率在光纤陀螺敏感轴方向上的分量，为地球自转角速 率，炉为当地纬度，θ为光纤陀螺敏感轴与北向的夹角，ε (t)为光纤陀螺随机漂移，其表达 式为 式中，ε ο为光纤陀螺常值漂移，G(t)为高斯白噪声。
3.根据权利要求1所述的一种用于光纤陀螺寻北的粒子滤波方法，其特征在于的步骤(2)中所述建立非线性光纤陀螺寻北系统的状态空间模型和观测模型的方法如下寻北时将光纤陀螺固定在水平转台上，此时为位置1，转台水平旋转180°，此时为位 置2，将水平面内相差180°的两个位置上的光纤陀螺输出量相减，构成二位置量测 ~ ω = ωκ cosO) cos(0k ) + ε\- ωκ cos(^) cos(0k +π)-ε\ =2iy(e cosO) cos(《 ) + ε\- ε] (3)式中，ε 为k时刻即第k次滤波在位置1处光纤陀螺的输出，￡k2为k时刻在位置2 处光纤陀螺的输出，θ k为光纤陀螺敏感轴与北向的夹角在k时刻的值，Sk1为k时刻在位 置1处光纤陀螺的随机漂移，ε k2为k时刻在位置2处光纤陀螺的随机漂移； 建立系统的状态方程和观测方程 0k= L+WH(4) 式中，θ 为k-1时刻光纤陀螺敏感轴与北向的夹角，Wlri为k-Ι时刻平台振动等原因 引起的系统状态方程随机误差，Zk为k时刻的二位置观测量，Vk为k时刻陀螺漂移引起的 观测方程随机误差。
4.根据权利要求1所述的一种用于光纤陀螺寻北的粒子滤波方法，其特征在于步骤(3)中所述对后验概率密度函数进行混合高斯近似的方法如下对概率密度函数进行网格划分，将离散的滤波密度转化为连续的滤波密度。一个简单的处理方法便是高斯近似，其均值与方差可以通过计算粒子点的均值与方差获取。 式中，μ k为k时刻滤波密度的均值，Ai为k时刻粒子i的权值，Xki为k时刻粒子i的 值，Σ k为k时刻滤波密度的协方差，N为大于1的自然数。
5.根据权利要求4所述的一种用于光纤陀螺寻北的粒子滤波方法，其特征在于步骤(3)中采用混合高斯模型近似 例，μ j, Σ j, j = 1,2,3,...，N为混合高斯模型中第j个高斯分布的均值和协方差。
6.根据权利要求1所述的一种用于光纤陀螺寻北的粒子滤波方法，其特征在于的步骤(4)中所述连续滤波密度的网格近似的方法如下在状态空间是离散并有界的情况下，即状态空间由有限个离散状态xkJ，j = 1,2...N 组成，该离散状态取为网格的中心点，在k-1时刻状态可以取上述N个状态中的任何一个， 对于每个状态用表示每个状态的权值，当网格均勻分布时，Wk^j取网格中心点的 似然值，则k-Ι时刻的概率密度函数可表示为 式中，s ( ·)为脉冲函数，Xh为k-Ι时刻状态变量，Z1K为1时刻到k-Ι时刻观测量。
7.根据权利要求1所述的一种用于光纤陀螺寻北的粒子滤波方法，其特征在于的步骤(5)中所述构建基于网格的粒子滤波器的方法如下得到滤波密度后，对其进行高斯或混合高斯近似，将离散的滤波密度转换为连续的滤 波密度，并对该滤波密度进行网格近似，将获取的具有权值Wlri的格子作为新的粒子进入下 一次滤波循环；网格粒子滤波权值计算递推过程如下根据Chapman-KoImogrov方程可以得到先验概率密度 式中，Xk为k时刻状态变量。由于系统在k-Ι时刻的后验概率密度可由(14)式表示，则有 后验概率密度可通过Bayes规则得到 式中，z1:k为1时刻到k时刻观测量。利用k-Ι时刻粒子从建议概率密度中采样，由于k-Ι时刻后验概率密度利用网格进行 近似，粒子权值为Wk+因此有 式中，q( ·)为建议密度函数， 为k_l时刻第i个粒子的权值。则有 式中，^为未归一化时，第i个粒子在k时刻的权值。则权值计算公式为 归一化权值 式中，Wki为k时刻第i个粒子的权值。
8.根据权利要求1或6所述的一种用于光纤陀螺寻北的粒子滤波方法，其特征在于采 用所述粒子滤波器的基于网格的粒子滤波方法的步骤如下 Stepl 初始化，在ρ (x0)抽取N个样本点χΛ相应权值1/Ν ； St印2 抽样·< -q(xk I Xk^l,Zk) = Pixk I Ζλ±_λ)； St印3 计算权值并归一化，对OCHJ(SK) ^ =义； St印4 计算估计值，4； St印5 对离散的滤波密度进行高斯近似或重采样后利用EM算法进行混合高斯近似； St印6 对连续的滤波密度在一定的截断区域内进行网格划分； St印7 计算每个网格的权值，取网格中心点似然函数的值为新的权值W ； StepS 将带权值的网格点作为新的粒子X1/进行下一周期的滤波； 其中，X0为滤波初值，Xoi为滤波初始粒子，A为k时刻的滤波估值，wk\ wkJ为k时刻粒 子的权值和网格近似后的权值，X1/，xkJ为k时刻粒子的值和网格中心点的值，i，j = 1，2， 3......N。
全文摘要
本发明公布了一种用于光纤陀螺寻北的粒子滤波方法，首先建立单轴光纤陀螺敏感地球自转角速率水平分量的输出特性；然后根据所选光纤陀螺寻北方案建立非线性光纤陀螺寻北系统的状态空间模型和观测模型；对后验概率密度函数进行混合高斯近似；对连续滤波密度进行网格近似；构建基于网格的粒子滤波器；最后由导航计算机根据光纤陀螺寻北系统的状态方程和观测方程，采集光纤陀螺输出信息，并完成寻北的滤波解算。本发明寻北精度高。
文档编号G01C19/72GK101907460SQ20101010972
公开日2010年12月8日 申请日期2010年2月10日 优先权日2010年2月10日
发明者于明清, 刘建业, 周翟和, 熊剑, 熊智, 赖际舟, 赵伟, 钟丽娜 申请人:南京航空航天大学