基于自适应upf的sins大方位失准角初始对准方法
【技术领域】
[0001] 本发明涉及一种基于自适应UPF的SINS大方位失准角初始对准方法，属于导航技 术领域。
【背景技术】
[0002] 随着导航系统应用领域的不断拓展，大多数应用环境不能满足初始失准角为大 角度和噪声为高斯白噪声的条件，此时继续使用传统导航系统线性化模型和KF(Kalman Filter，卡尔曼滤波）将会产生较大的模型误差和估计误差，使得导航参数不可信。针对这 种情况，国内外的研究主要分为两个方面：一是惯性导航系统的非线性模型的研究，二是非 线性滤波器的研究。常用的非线性滤波方法有EKF(ExtendedKalmanFilter，扩展卡尔曼 滤波）、UKF(UnscentedKalmanFilter，无迹卡尔曼滤波）、PF(ParticleFilter，粒子滤 波）、EKF-PF、UKF-PF(UPF)，在一定程度上UPF的使用限制更少、滤波效果要优于其他几种。
【发明内容】
[0003] 发明目的：本发明的目的在于针对大多数应用环境不能满足初始失准角为大角度 和噪声为高斯白噪声的条件以及UPF的不足进行改进，本发明提出一种基于强跟踪滤波器 技术的自适应UPF的大方位失准角初始对准的方法。本发明将UPF和自适应UPF两种非线 性滤波器在初始对准中的滤波效果进行了比较，结果表明自适应UPF比UPF具有更快的收 敛速度并且在一定程度上提高了对准精度。本发明的方法是通过引入衰减记忆因子有效增 强当前信息残差对系统修正作用，在一定程度上降低了由于系统简化、噪声统计特性不确 定对系统造成的影响，同时较好的克服了UPF中粒子退化的现象。
[0004] 技术方案：本发明所述的基于自适应UPF的SINS大方位失准角初始对准方法，包 括以下步骤：
[0005] 步骤1:建立SINS非线性误差模型：
[0006] 使用欧拉平台误差角来表示理想导航坐标系与计算导航坐标系之间的失准角，且 该组误差角需要考虑转动的先后顺序，建立相应的SINS非线性误差模型；
[0007] 本发明坐标系选取如下：
[0008]i系--地心惯性坐标系，原点位于地心0，X;轴指向春分点，z;轴沿地球自转轴， 轴与Xi、Zl构成右手系；
[0009]e系--地球坐标系，原点位于地心，xe轴穿越本初子午线与赤道的交点，ze轴穿 越地球北极点，轴穿越东经90°子午线与赤道的交点；
[0010] n系 导航坐标系，这里选取"东-北 -天（E_N_U) "地理坐标系为导航坐标系；
[0011] b系 "右-前-上"坐标系为载体坐标系（b系）；
[0012] n系先后依次经过三次欧拉角转动至b系，这三个欧拉角分别记为航向角 边G(-31 31]、俯仰角0G[-31/2 31/2]和横滚角YG(-31 31]，n系与b系之间的旋 转变换关系可用姿态矩阵cr描述；
[0013] 步骤2:SINS大方位失准角初始对准误差模型：
[0014] 现假设两水平失准角都为小角度；假设陀螺测量误差主要为常值漂移误差 eb和零均值高斯白噪声加速度计测量误差主要为常值偏置误差和零均值高斯 白噪声忽略重力误差项sgn，静基座下>? = 成立，则初始对准滤波模型的状态方 程为：
[0015] 丨V、二U
[0016] 令状态向量#/ (加f(AMvYf:,噪声向量价# 滤波状态模型，并直接以SINS速度误差z=S，作为观测量建立观测方程如下：
[0017]
[0018] f(x)和g(x)的具体表达式参考上式的初始对准滤波模型的状态方程，H= [0I 0 0]，v为量测噪声；
[0019] 步骤3 :标准UPF算法：
[0020] 假设初始状态变量X。~p(X。)，方差阵为P。，对观测方程进行无迹粒子滤波，算法 如下：
[0021] 步骤3. 1:初始化：k=0 ;从初始的先验概率分布p(x。）中进行粒子；^4)，i= 1， 2,3."』的采样，为简化计算，令；^~1(艺'4/)，其中斤1：=扃' /^=4;[0022] 步骤3.2:加权粒子的预测、采样：k= 1，2,利用无迹卡尔曼滤波对粒子进行预测更新，并计算〇样本点：[0023]
[0024]
[0025]
[0026] 式中，;/ ：=：^Jn+X，入=a2(n+i〇-n，10a< 1，k= 3_n，< :为估计均 方误差，n为状态维数；
[0027] 时间更新为： 「00281
[0033]入=a2(n+K)-n，10a彡 1，k= 3-n，对于正态分布而言，# =2,/g4 为 预测均方差，W为权值，Z为量测量，K为滤波增益；
[0034] 量测更新为：
[0035]
[0036] 以推荐密度函数wdga)，々)）忽V(艺，]，/^)生成粒子;^作为二次采样 的原始粒子；
[0037]步骤3. 3:根据权值更新公式
对N个粒子相应的权值进行计算并做归一化处理；其中， (八~ "二、VM .￡，：
；
[0038] 步骤3. 4 :通过重采样算法对原始粒子(i= 1，2,…，N)作二次采样生成二次 采样粒子(j= 1，2,…，N)，并计算每个二次采样粒子的权重系数；
[0039] 步骤3. 5 :根据t 计算状态变量的最优估计及每个粒子对应的方差 i:=i 矩阵；
[0040] 步骤3. 6 :将步骤3. 4重采样后的粒子尤°及步骤3. 5计算的if1代入第2步进行 迭代运算；
[0041] 步骤4 :自适应UPF算法：
[0042] 基于强跟踪滤波的思想，采用时变渐消因子削弱陈旧数据对当前滤波值的影响， 实时调整状态预测误差的协方差以及相应的增益矩阵来达到。这在一定程度上能够减缓 UPF粒子退化的现象，并且加快粒子滤波的收敛速度。
[0043] 本发明采取的自适应措施是对滤波器的协方差进行判断，具体判断公式如下：
[0044]
[0045] 其中，S为设定的调整系数，一般取S
> 1，此处取S= 1. 5 ; &为系统的残差序列， 4 二A-hCD;
[0046] 当上式的判断公式不成立时，需要对巧；L:进行有效修正，本发明采用的方式是引 入哀减记忆因子的自适应加权系数Ak，其具体的定义如下：
[0047]
[0048] 修正公式为：[0049]
[0050]
[0051]
[0052]
[0053] 式中，P-般取0<P< 1，其主要作用是增强滤波器对系统状态的快速跟踪能 力，其越大，则当前信息分配的权值也越大，当前信息的残差对系统估计的影响也更突出。 为了保证系统对缓慢变化情况和突变状态的强跟踪能力，本发明取P= 〇. 95 ;
[0054] 进一步地，步骤1具体包括：
[0055] 步骤1. 1 :建立姿态误差方程
[0056] 实际工作的导航系统由于存在各种干扰和量测误差，SINS的计算的导航坐标系 (n'系）通常与理想的导航坐标系（n系）之间存在转动误差，n系需按照一定的顺序依次 转过三个角度才能与n'系重合，现假设这三次转动依次绕z轴、x轴、y轴旋转，且转过的 角度分别记为0、A、％，其矢量表示形式为P= %A]、三次旋转对应的姿 态变换阵依次为、Cs._、. 故n系至n'系的变换矩阵为：
[0057]
[0058
[0059] 若设n'系相对于n系的角速度为，则与欧拉平台误差角@的关系可表示 如下：
[0060]
[0061] 则由上式可得欧拉平台误差角微分方程：
[0062]
[0065] 在导航坐标系中，SINS的姿态矩阵微分方程表示如下：
[0066]
[0067] 式中，运算符（?X)表示由向量?构成的反对称矩阵，符号表示坐标系c相对 于坐标系b的运动向量在坐标系d中的投影，符号兮;表示b系至c系的变换矩阵，而实际 上用于姿态更新的微分方程是带有误差的，其形式可以表示为：
[0068]
[0069] 式中，符号"~"表不测量值，私。为成的测量 值，为两；：的计算误差，为陀螺测量误差；令理论姿态矩阵与实际姿态矩阵的解算 误差为：
[0070]
[0071] 式中，I表示单位矩阵，对AC=Q两边同时求导，并将SINS的姿态矩阵微 分方程和实际用于姿态更新的微分方