CN112649022B - 一种考虑挠曲变形和杆臂效应的大失准角传递对准方法 - Google Patents

Abstract

本发明提出一种考虑挠曲变形和杆臂效应的大失准角传递对准方法。首先，推导了惯性系下挠曲变形和杆臂效应一体化误差方程。然后，建立了基于罗德里格参数的非线性传递对准模型。最后，然后根据模型的部分非线性特征，设计边缘平方根容积卡尔曼滤波器，通过平方根滤波进行线性时间更新，通过平方根容积卡尔曼滤波进行非线性量测更新，实现失准角的估计。本发明可以有效补偿挠曲变形及动态杆臂误差，提高对准精度，并且具有稳定的滤波特性以及良好的计算实时性。

Description

一种考虑挠曲变形和杆臂效应的大失准角传递对准方法

技术领域

本发明涉及捷联惯导技术领域，特别是涉及一种考虑挠曲变形和杆臂效应的大失准角传递对准方法。

背景技术

随着现代战争对武器装备精确打击能力和快速反应能力的要求日益提高，高精度快速对准技术对于战术级武器的导航系统愈加重要。在舰载环境下，利用高精度主惯导信息进行对准的传递对准方法，凭借其高精度及快速性的优势，成为子惯导系统对准的主要方案。

在实际传递对准工作过程中，由于主、子惯导安装位置不同导致的杆臂效应作为影响对准精度的重要误差因素，在固定杆臂长度已知的情况下，通常在滤波器量测信息中进行补偿。然而由于载体并非理想刚体，在外界环境如日晒、海浪冲击等影响下，会发生挠曲变形。载体的挠曲变形一方面会对惯性测量器件的输出产生影响，另一方面会引起杆臂长度的变化，产生动态杆臂误差，无法在量测量中有效补偿。因此，为提高传递对准性能，需要同时考虑挠曲变形以及动态杆臂误差影响。

徐晓苏等人在《基于鲁棒滤波的挠曲变形和动态杄臂补偿算法》(发表于《中国惯性技术学报》，2015年，第23卷，第1期)一文中，建立了挠曲变形和杆臂效应一体化传递对准误差模型，改善了随机扰动环境下的失准角估计效果。然而上述研究对失准角进行了小角度假设，将模型进行了线性化处理。而实际情况中，舰船主惯导系统一般固定安装于舰船甲板下方的导航室内，而舰载机却可能停靠在甲板上任意位置，导致主子惯导间的失准角可能较大，无法适用基于小失准角的线性对准误差模型。因此需要针对大失准角情况下的非线性传递对准一体化误差模型进行研究。

高青伟等人在《大方位失准角传递对准非线性模型研究》(发表于《控制与决策》，2011年，第26卷，第3期)一文中建立了大方位失准角条件下挠曲变形和杆臂效应一体化的非线性传递对准模型，但该模型没有对实际失准角进行建模，并且未能实现组合匹配对准。卢航等人在《基于边缘采样的简化高阶CKF在非线性快速传递对准中的应用》(发表于《航空学报》，2019年，第40卷，第3期)一文中建立了基于速度加姿态的组合匹配非线性快速传递对准一体化模型，但以欧拉角作为大失准角的姿态描述，会受到奇异性及超越函数的限制。段腾辉在《光纤捷联惯导系统快速对准技术研究》(哈尔滨工程大学，2019年)中推导了基于四元数的大失准角快速传递对准模型，该模型具有非奇异性、对大角度旋转描述更精确等优点，但由于四元数的冗余性质增加了计算量，同时滤波时也很难兼顾四元数的单位模约束。Cui X等人在《A unified model for transfer alignment at random misalignmentangles based on second-order EKF》(发表于《Measurement ence&Technology》，2017年，第28卷，第4期)一文中提出了在惯性系框架内进行传递对准的思路，建立了基于罗德里格参数的大失准角传递对准模型，但是并没有考虑挠曲变形和杆臂效应的综合影响。

因此，为解决随机扰动环境下的大失准角传递对准问题，本发明提出一种考虑挠曲变形和杆臂效应的大失准角传递对准方法。该方法推导惯性系下挠曲变形和杆臂效应一体化误差方程，建立基于罗德里格参数的非线性传递对准模型，并根据模型的部分非线性特征，设计边缘平方根容积卡尔曼滤波器，实现失准角的滤波估计。

发明内容

本发明的目的在于提供一种可应用于挠曲变形以及杆臂效应影响下的舰船大失准角高精度快速传递对准方法。

实现本发明目的的技术方案为：一种考虑挠曲变形和杆臂效应的大失准角传递对准方法，包括以下步骤：

步骤一：在运载体上安装高精度主惯导系统和精度较低的子惯导系统，完成启动、预热准备；

步骤二：计算主、子惯导的载体坐标系至惯性凝固坐标系之间的转换矩阵，通过该矩阵将主、子惯导的比力及角速度信息投影至惯性凝固坐标系，然后将投影后的主惯导比力及角速度传输到子惯导计算机。

步骤三：将主、子惯导惯性凝固坐标系下的比力进行积分，选取主、子惯导之间的比力积分差值、角速度差值作为量测信息，构建基于罗德里格参数的挠曲变形和动态杆臂效应一体化非线性传递对准模型。

步骤四：利用边缘平方根容积卡尔曼滤波器进行滤波，估计子惯导失准角，修正子惯导姿态，完成传递对准。

在步骤三中，构建基于罗德里格参数的挠曲变形和动态杆臂效应一体化非线性传递对准模型，具体方法为：

模型状态向量选取为

各个状态的状态方程为

式中，

其中，

为与失准角对应的罗德里格参数；ε_s为子惯导陀螺常值漂移；

为加速度计常值零偏；

为i_m系下的动态杆臂加速度积分；θ＝[θ_x θ_y θ_z]^T为挠曲变形角；μ＝[μ_x μ_yμ_z]^T为挠曲变形角速率；β＝2.146/τ，τ＝[τ_x τ_y τ_z]^T为相关时间；w_f为激励白噪声,其方差满足Var(w_f)＝Q_f＝4β³σ²，其中σ＝[σ_x σ_y σ_z]^T为挠曲变形角方差。

根据上述状态方程，构建系统状态模型

式中，过程噪声

状态转移矩阵F表示为

其中，

A_w1＝diag(-β²)，A_w2＝diag(-2β)。

过程噪声驱动矩阵G表示为

量测方程为

式中，

分别为

及

的积分，w_k表示由主惯导刚性杆臂补偿残余误差、惯性器件误差的积分引起的量测噪声。

在步骤四所述的利用边缘平方根容积卡尔曼滤波器进行滤波，具体方法为：

1)时间更新过程

计算状态一步预测

以及状态一步预测均方误差阵平方根因子S_k/k-1

S_k/k-1＝Tria([F_k/k-1S_k-1 S_Q,k-1])

式中，Tria(·)表示经QR分解获得的下三角矩阵，S_Q,k-1表示过程噪声矩阵Q_k-1的平方根因子，即

2)量测更新过程

根据

及S_k/k-1计算容积点

式中，ξ_i表示ξ的第i列，

I_n表示n阶单位矩阵。

容积点经过非线性量测方程传播，计算量测预测容积点

计算量测预测值

计算新息协方差矩阵的平方根因子

S_zz,k/k-1＝Tria([η_k/k-1S_R,k])

式中，

S_R,k表示R_k的平方根因子，即

计算互协方差矩阵

式中

计算滤波增益

计算状态估计

计算状态估计均方误差阵的平方根因子

S_k＝Tria([χ_k/k-1-K_kη_k/k-1 K_kS_R,k])

与现有技术相比，本发明的有益效果是：

本发明在舰船甲板存在随机扰动的情况下，建立了基于罗德里格参数的挠曲变形和动态杆臂效应一体化非线性传递对准模型，并根据模型的部分非线性特征，设计边缘平方根容积卡尔曼滤波器，有效补偿挠曲变形及动态杆臂误差，提高随机扰动影响下的对准精度，并且具有稳定的滤波特性以及良好的计算实时性。

附图说明

图1为本发明方法流程示意图；

图2为由x轴挠曲变形角引起的动态杆臂长度变化示意图；

图3为挠曲变形角估计；

图4为挠曲变形角估计误差；

图5为失准角估计误差均值；

图6为失准角估计误差RMS；

图7为滤波器滤波的周期；

具体实施方式

以下结合附图1本发明方法流程示意图，对本发明进行详细说明。

为了验证本发明的有效性，利用Matlab对本发明的方法进行仿真。

1)推导惯性系下挠曲变形和杆臂效应一体化误差方程，具体步骤如下：

载体的动态挠曲变形是随机扰动量，而当前时刻的扰动位置与之前时刻有关，与白噪声驱动的马尔可夫过程类似，因此通常把载体的动态挠曲变运动形视为马尔可夫过程。考虑到高阶模型的计算量较大，对实际工程中计算机能力要求更高，并参考近些年的相关研究，本文将动态挠曲变形考虑为二阶马尔可夫过程：

式中，θ＝[θ_x θ_y θ_z]^T为挠曲变形角；μ＝[μ_x μ_y μ_z]^T为挠曲变形角速率；β＝2.146/τ，τ＝[τ_x τ_y τ_z]^T为相关时间；w_f为激励白噪声,其方差满足Var(w_f)＝Q_f＝4β³σ²，其中σ＝[σ_x σ_y σ_z]^T为挠曲变形角方差。

由于受到挠曲变形的影响，主、子惯导之间的固定杆臂长度发生动态变化，产生了动态杆臂误差。设主惯导载体坐标系(m系)下固定杆臂矢量r₀＝[r_x r_y r_z]^T，挠曲变形角矢量θ＝[θ_x θ_y θ_z]^T，并考虑为小角度。图2表示由x轴挠曲变形角θ_x引起的动态杆臂长度变化示意图，则由图2可知，θ_x对杆臂分量产生的变化量为

同理，θ_y以及θ_z对杆臂分量产生的变化量为δr_x1＝r_zθ_y，δr_z1＝-r_xθ_y，δr_x2＝-r_yθ_z，δr_y2＝r_xθ_z，因此动态杆臂r^m可表示为

动态杆臂矢量的一阶、二阶导数分别为

杆臂效应影响下主、子惯导敏感的比力在地心惯性系(i-frame)内的投影关系为

式中，

表示主惯导坐标系至地心惯性坐标系的坐标变换矩阵，

表示主惯导陀螺输出，

表示刚性杆臂引起的加速度；

是由于动态杆臂引起的加速度。

结合式(1)至式(5)，并补偿刚性杆臂加速度后，则有

挠曲变形影响下，惯性系内主、子惯导陀螺仪测量的角速度关系为

式中，

表示子惯导陀螺测量值在惯性系下的投影，

表示主惯导陀螺仪测量值，μ表示挠曲变形角速度。

2)建立基于罗德里格参数的主子惯导测量方程，具体方法为：

将传递对准初始时刻的主惯导载体坐标系(m)和子惯导载体坐标系(s)进行固定，分别得到惯性凝固坐标系i_m和i_s。由于对准过程中m系相对于i_m系转动，导致两者之间的坐标变换矩阵

为时变矩阵，该矩阵可由主惯导陀螺仪输出

计算得到。考虑主惯导器件精度较高，在忽略其误差的情况下，根据方向余弦矩阵微分方程，则有

同理，s系与

坐标变换矩阵

可表示为

式中，

表示计算子惯导惯性凝固坐标系，

表示子惯导陀螺仪输出。

由于受子惯导器件误差的影响，导致

无法准确跟踪理想子惯导惯性凝固坐标系i_s，在短时间对准过程中，该跟踪误差

可视为小量，则有

式中，ε^s表示子惯导陀螺仪常值漂移，

表示子惯导陀螺仪随机噪声。

在测得主、子惯导准确固定杆臂长度后，对主惯导进行刚性杆臂加速度补偿，记补偿后比力为

利用式(8)计算得到的m系至i_m系坐标变换矩阵

将

投影至i_m系中，即

同理，将子惯导加速度计输出由

投影至

系中

式中，

表示子惯导加速度计常值零偏，

子惯导加速度计随机噪声。

将上式进行展开，并忽略高阶小量，则有

结合式(6)、式(11)、式(13)，则有

式中，

为常值矩阵，表示惯性凝固坐标系i_m至i_s的坐标变换矩阵，即传递对准初始时刻主、子惯导载体坐标系之间的安装误差角对应的方向余弦矩阵。

同理，对于主、子惯导陀螺仪输出则有

式中

将

以罗德里格向量

表示，根据Cayley变换，两者之间满足如下关系式

将式(16)带入分别式(14)、式(15)，经过整理可得

式中，

式(17)、式(18)等式右边包含了主、子惯导间的安装误差角对应的罗德里格向量

因此可采用主、子惯导的比力

以及角速度

作为匹配量，构建量测方程。由于在推导过程中未对安装误差角进行小角度假设，因此该匹配方程适用于大失准角的传递对准过程。

3)构建传递对准滤波模型，具体方法为：

子惯导IMU的比力及角速度输出中包含了动态挠曲变形、测量噪声等随机噪声扰动项，影响式(17)、式(18)中的匹配量精度。因此可以通过积分过程的平滑特性衰减随机噪声，提高量测信息的信噪比，改善失准角估计性能。然而，积分操作虽然可以一定程度上提高对于随机噪声的抗扰能力，但同时削弱了量测信息与状态之间的关联程度，降低了状态的可观测性。因此，在通过积分操作衰减随机噪声影响的同时，还需要兼顾状态的可观测性。本发明仅选择惯性凝固坐标系下的主、子惯导比力进行积分。

对式(17)进行积分，则有

式中，

由于对准过程中

为常值矩阵，则与

对应的罗德里格参数

满足如下微分方程

考虑子惯导陀螺常值漂移ε_s以及加速度计常值零偏

并将i_m系下的动态杆臂加速度积分

挠曲变形角θ及其角速度μ等列入状态量，则状态向量选取为

则根据式(1)、式(10)、式(19)、式(20)，构建系统状态微分方程

式中，过程噪声

状态转移矩阵F表示为

其中，

A_w1＝diag(-β²)，A_w2＝diag(-2β)

过程噪声驱动矩阵G表示为

根据式(18)、式(19)，构建量测方程

式中，w_k表示由主惯导刚性杆臂补偿残余误差、惯性器件误差的积分引起的量测噪声。

4)设计边缘平方根容积卡尔曼滤波，具体方法为：

所建立的滤波模型的状态方程为线性，量测方程为非线性基于模型的该特性，仅对滤波器非线性部分，即量测更新过程，采用容积点采样的方式，通过SRCKF进行更新；而对滤波器线性部分，即时间更新过程，直接采用线性映射方式，通过平方根滤波(SRF)进行更新。

a.时间更新过程

计算状态一步预测

以及状态一步预测均方误差阵平方根因子S_k/k-1

S_k/k-1＝Tria([F_k/k-1S_k-1 S_Q,k-1]) (25)

式中，Tria(·)表示经QR分解获得的下三角矩阵，S_Q,k-1表示过程噪声矩阵Q_k-1的平方根因子，即

b.量测更新过程

根据

及S_k/k-1计算容积点

式中，ξ_i表示ξ的第i列，

I_n表示n阶单位矩阵。

容积点经过非线性量测方程传播，计算量测预测容积点

计算量测预测值

计算新息协方差矩阵的平方根因子

S_zz,k/k-1＝Tria([η_k/k-1 S_R,k]) (29)

式中，

S_R,k表示R_k的平方根因子，即

计算互协方差矩阵

式中

计算滤波增益

计算状态估计

计算状态估计均方误差阵的平方根因子

S_k＝Tria([χ_k/k-1-K_kη_k/k-1 K_kS_R,k]) (33)

5)最后，仿真验证本发明的有效性。

a.仿真条件设置

式中，θ、γ、ψ分别表示舰船纵摇、横摇、航向角；θ_m、γ_m、ψ_m分别表示舰船摇摆幅度；ω_y，ω_p，ω_r表示舰船摇摆角频率，T_i＝2π/ω_i,(i＝y,p,r)为摇摆周期；θ₀、γ₀、

表示舰船初始相位；K表示初始航向。

仿真参数设置如下：

摇摆角幅值：ψ_m＝8°，θ_m＝10°，γ_m＝7°；

摇摆周期：T_y＝8s，T_p＝12s，T_r＝6s；

初始相位：θ₀＝8°，γ₀＝0°，ψ₀＝45°；

初始航向：K＝30°；

初始纬度φ＝45.7796°，初始经度λ＝126.6705°；

舰船以10n mile/h的速度匀速直航；

陀螺仪常值漂移为0.01°/h，角度随机游走为

加速度计常值偏置为10μg，加速度计随机漂移为

固定杆臂长度为r₀＝[5m 5m 5m]^T；

二阶马尔可夫过程的动态挠曲变形角相关时间τ＝[30s 30s 30s]^T；

挠曲变形角方差σ²＝[10' 10' 10']^T；

主子惯导之间的安装误差角(实际失准角)φ＝[5° 5° 60°]^T；

滤波器时间更新周期t_n＝0.01s，量测更新周期t_m＝0.1s；

滤波器状态初始值X(0)＝0_24×1

系统过程噪声矩阵Q根据陀螺随机噪声、加速度计随机噪声、挠曲变形角的二阶马尔可

夫激励白噪声设置，其协方差矩阵分别为：

Q_a＝diag[(1μg)² (1μg)² (1μg)²]，

Q_f＝diag[4·(2.146/30s)³·10' 4·(2.146/30s)³·10' 4·(2.146/30s)³·10']；

量测噪声协方差矩阵R根据比力量测噪声、比力积分噪声以及加角速度量测噪声设置，

其协方差矩阵分别为：

R_a＝diag[(1μg)²(1μg)²(1μg)²]，

b.仿真结果

首先通过所提方案对挠曲变形进行估计，验证所提传递对准方案对挠曲变形的补偿作用。根据仿真条件，通过所提方案对挠曲变形角θ及角速度μ进行估计。图3和图4分别表示挠曲变形角的估计值以及相应估计误差。由图可知，滤波器随着滤波过程逐渐收敛，θ估计值逐渐逼近真值。在滤波收敛后(40s～200s)估计误差RMS为[1.4473',1.1811',0.6870']^T，表明滤波器可以较为准确的跟踪挠曲变形角，验证了所提方案对抑制挠曲变形角的有效性。

然后通过与其它已有方法的比较验证所提传递对准方案的有效性。方法1采用CuiX等人建立的未考虑挠曲变形与杆臂效应的大失准角传递对准模型，通过常见的容积卡尔曼滤波(Cubature Kalman Filter,CKF)进行滤波估计；方法2采用本文建立的传递对准模型，通过常见的CKF进行滤波估计。

为准确的比较各方案的对准效果，根据仿真条件进行50次蒙特卡洛仿真，仿真结果如图5和图6所示。由图可知，对于φ_z，方法1估计误差RMS为12.2231'，而方法2估计误差RMS为6.2912'，与方法1相比，在提高收敛速度的同时，对准精度提升50.62％。同理，对于φ_x、φ_y，具有相同的仿真结果。这表明在采用相同滤波器(CKF)的情况下，方法2较方法1具有更优的对准性能，验证了所建立的传递对准模型的有效性以及优越性。

进一步，对采用相同滤波模型的方法2和所提方法进行比较。CKF(方法2的滤波器)和MSRCKF(所提方法的滤波器)的滤波时间及其统计结果分别如图7所示。由图可知，CKF的滤波周期均值为1.5892E-3s，相同的滤波模型下，MSRCKF的滤波周期均值为1.3598E-3s，与CKF相比，滤波时间较CKF减少了14.36％。由图5和图6可知，所提方法与方法2具大致相同的对准精度，因此可知，MSRCKF表明在不损失滤波精度的前提下，具有更优的计算实时性，验证了所设计滤波器的有效性。

综合上述所述，本发明提供的方法可以有效抑制挠曲变形及杆臂误差，改善大失准角对准精度，提高对准过程计算实时性以及稳定性。

Claims (1)

1.一种考虑挠曲变形和杆臂效应的大失准角传递对准方法，其特征在于，包括以下步骤：

步骤一：在运载体上安装高精度主惯导系统和精度较低的子惯导系统，完成启动、预热准备；

步骤二：计算主、子惯导的载体坐标系至惯性凝固坐标系之间的转换矩阵，通过该矩阵将主、子惯导的比力及角速度信息投影至惯性凝固坐标系，然后将投影后的主惯导比力及角速度传输到子惯导计算机；

步骤三：将主、子惯导惯性凝固坐标系下的比力进行积分，选取主、子惯导之间的比力积分差值、角速度差值作为量测信息，构建基于罗德里格参数的挠曲变形和动态杆臂效应一体化非线性传递对准模型；

步骤四：利用边缘平方根容积卡尔曼滤波器(Marginalized Square-root CubatureKalman Filter,MSRCKF)进行滤波，估计子惯导失准角，修正子惯导姿态，完成传递对准；

步骤三所述的构建基于罗德里格参数的挠曲变形和动态杆臂效应一体化非线性传递对准模型，具体方法为：

模型状态向量选取为

各个状态的状态方程为

式中，

其中，

为与失准角对应的罗德里格参数；ε_s为子惯导陀螺常值漂移；

为加速度计常值零偏；

为i_m系下的动态杆臂加速度积分；θ＝[θ_x θ_y θ_z]^T为挠曲变形角；μ＝[μ_x μ_y μ_z]^T为挠曲变形角速率；β＝2.146/τ，τ＝[τ_x τ_y τ_z]^T为相关时间；w_f为激励白噪声,其方差满足Var(w_f)＝Q_f＝4β³σ²，其中σ＝[σ_x σ_y σ_z]^T为挠曲变形角方差；

根据上述状态方程，构建系统状态模型

式中，过程噪声

状态转移矩阵F表示为

其中，

A_w1＝diag(-β²)；A_w2＝diag(-2β)；

过程噪声驱动矩阵G表示为

量测方程为

式中，

分别为

及

的积分；w_k表示由主惯导刚性杆臂补偿残余误差、惯性器件误差的积分引起的量测噪声；

步骤四所述的利用边缘平方根容积卡尔曼滤波器进行滤波，具体方法为：

1)时间更新过程

计算状态一步预测

以及状态一步预测均方误差阵平方根因子S_k/k-1

S_k/k-1＝Tria([F_k/k-1S_k-1 S_Q,k-1])

式中，Tria(·)表示经QR分解获得的下三角矩阵，S_Q,k-1表示过程噪声矩阵Q_k-1的平方根因子，即

2)量测更新过程

根据

及S_k/k-1计算容积点

式中，ξ_i表示ξ的第i列，

I_n表示n阶单位矩阵；

容积点经过非线性量测方程传播，计算量测预测容积点

计算量测预测值

计算新息协方差矩阵的平方根因子

S_zz,k/k-1＝Tria([η_k/k-1 S_R,k])

式中，

S_R,k表示R_k的平方根因子，即

计算互协方差矩阵

式中

计算滤波增益

计算状态估计

计算状态估计均方误差阵的平方根因子

S_k＝Tria([χ_k/k-1-K_kη_k/k-1 K_kS_R,k])。

Images