基于自适应UPF的SINS大方位失准角初始对准方法

摘要

本发明公开一种基于自适应UPF的SINS大方位失准角初始对准方法，包括以下步骤：步骤1：建立SINS非线性误差模型：使用欧拉平台误差角来表示理想导航坐标系与计算导航坐标系之间的失准角，且陔组误差角需要考虑转动的先后顺序，建立相应的SINS非线性误差模型；本发明解决了在一定程度上降低了由于系统简化、噪声统计特性不确定对系统造成的影响，同时较好的克服了UPF中粒子退化的现象，提高了捷联惯性导航系统初始对准精度且减少了对准时间。本发明为软件方法，不需要对系统硬件做任何修改，故实际实施方便、可行。

主权项

一种基于自适应UPF的SINS大方位失准角初始对准方法，其特征为：步骤1：建立SINS非线性误差模型：使用欧拉平台误差角来表示理想导航坐标系与计算导航坐标系之间的失准角，且该组误差角需要考虑转动的先后顺序，建立相应的SINS非线性误差模型；本发明坐标系选取如下：i系——地心惯性坐标系，原点位于地心0，x_i轴指向春分点，z_i轴沿地球自转轴，y_i轴与x_i、z_i构成右手系；e系——地球坐标系，原点位于地心，x_e轴穿越本初子午线与赤道的交点，z_e轴穿越地球北极点，y_e轴穿越东经90°子午线与赤道的交点；n系——导航坐标系，这里选取“东‑北‑天(E‑N‑U)”地理坐标系为导航坐标系；b系——“右‑前‑上”坐标系为载体坐标系(b系)；n系先后依次经过三次欧拉角转动至b系，这三个欧拉角分别记为航向角ψ∈(‑π π]、俯仰角θ∈[‑π/2 π/2]和横滚角γ∈(‑π π]，n系与b系之间的旋转变换关系可用姿态矩阵描述；步骤2：SINS大方位失准角初始对准误差模型：现假设两水平失准角都为小角度；假设陀螺测量误差主要为常值漂移误差ε^b和零均值高斯白噪声加速度计测量误差主要为常值偏置误差和零均值高斯白噪声忽略重力误差项δgⁿ，静基座下成立，则初始对准滤波模型的状态方程为：式中，和分别为SINS的理想的导航坐标系(n系)绕x轴、y轴和z轴转过的角度，为向量的导数，为SINS的理想的导 航坐标系(n系)至计算的导航坐标系(n′)的变换矩阵，为陀螺的常值漂移误差ε^b的导数，为加速计的常值偏置的导数，误差令状态向量噪声向量建立滤波状态模型，并直接以SINS速度误差z＝δvⁿ作为观测量建立观测方程如下：f(x)和g(x)的具体表达式参考上式的初始对准滤波模型的状态方程，H＝[0 I 0 0]，v为量测噪声；步骤3：标准UPF算法：假设初始状态变量x₀～p(x₀)，方差阵为P₀，对观测方程进行无迹粒子滤波，算法如下：步骤3.1：初始化：k＝0；从初始的先验概率分布p(x₀)中进行粒子i＝1，2，3…，N的采样，为简化计算，令其中步骤3.2：加权粒子的预测、采样：k＝1，2，...；利用无迹卡尔曼滤波对粒子进行预测更新，并计算σ样本点：式中，λ＝α²(n+κ)‑n，10^‑4≤α≤1，κ＝3‑n，为估计均方误差，n为状态维数；时间更新为：其中λ＝α²(n+κ)‑n，10^‑4≤α≤1，κ＝3‑n，对于正态分布而言，β＝2，为预测均方差，W为权值，Z为量测量，K为滤波增益；量测更新为：以推荐密度函数生成粒子作为二次采样的原始粒子；步骤3.3：根据权值更新公式对N个粒子相应的权值进行计算并做归一化处理；其中，步骤3.4：通过重采样算法对原始粒子作二次采样生成二次采样粒子并计算每个二次采样粒子的权重系数；步骤3.5：根据计算状态变量的最优估计及每个粒子对应的方差矩阵；步骤3.6：将步骤3.4重采样后的粒子及步骤3.5计算的代入第2步进行迭代运算；步骤4：自适应UPF算法：基于强跟踪滤波的思想，采用时变渐消因子削弱陈旧数据对当前滤波值的影响，实时调整状态预测误差的协方差以及相应的增益矩阵来达到；这在一定程度上能够减缓UPF粒子退化的现象，并且加快粒子滤波的收敛速度；以上采取的自适应措施是对滤波器的协方差进行判断，具体判断公式如下：其中，S为设定的调整系数，一般取S＞1，此处取S＝1.5；为系统的残差序列，当上式的判断公式不成立时，需要对进行有效修正，在此采用的方式是引入衰减记忆因子的自适应加权系数λ_k，其具体的定义如下：修正公式为：式中，Q为系统噪声方差，式中，ρ一般取0＜ρ≤1，其主要作用是增强滤波器对系统状态的快速跟踪能力，其越大，则当前信息分配的权值也越大，当前信息的残差对系统估计的影响也更突出。为了保证系统对缓慢变化情况和突变状态的强跟踪能力，在此取ρ＝0.95。