一种基于自适应补偿H无穷滤波的传递对准方法与流程

本发明涉及一种传递对准方法，具体来说，涉及一种基于自适应补偿h无穷滤波的传递对准方法。

背景技术：

初始对准对于捷联惯性导航系统而言至关重要。捷联惯性导航系统的性能很大一部分取决于初始对准的速度和精度。根据不同的对准模式，初始对准可分为自对准、组合对准和传递对准。相比于自对准和组合对准，传递对准过程更加快速，对准时间可以大大缩短。在传递对准中，当主惯导器件精度很高时，子惯导可以获得较高的对准精度。基于以上优势，传递对准在飞行器、舰船和车辆上得到了广泛的应用。

传统的传递对准方法主要使用卡尔曼滤波技术，但是在复杂动态条件下，系统的系统噪声和量测噪声为未知的非高斯白噪声。此时，若使用卡尔曼滤波技术，则系统状态量估计的准确性和传递对准的精度将不可避免的下降，甚至引起系统的发散。

基于此，相关文献提出了用h无穷滤波器来进行传递对准，可有效提高系统的鲁棒性和稳定性。但是h无穷滤波器的鲁棒因子的大小是根据经验来确定的，一旦确定就不再改变。而在整个传递对准过程中，外界环境是动态变化的。这就需要动态调整鲁棒因子的大小，因此在h无穷滤波器中固定鲁棒因子大小的方式在未知动态环境下具有一定的局限性。

技术实现要素：

技术问题：本发明提供一种基于自适应补偿h无穷滤波的传递对准方法，保证了传递对准过程中滤波器的稳定性，同时提高了传递对准精度。

技术方案：为解决上述技术问题，本发明实施例采用以下技术方案：

一种基于自适应补偿h无穷滤波的传递对准方法，包括：

步骤10)建立传递对准模型；

步骤20)对步骤10)建立的传递对准模型，进行自适应补偿h无穷滤波，估计失准角，完成传递对准。

作为优选例，所述的步骤10)包括：

步骤101)建立传递对准系统状态方程：

选取12维状态建立系统的状态向量x(t)，系统状态向量x(t)如式(1)所示：

其中，δl表示纬度误差，δλ表示经度误差,δve表示导航坐标系中的东向速度误差，δvn表示导航坐标系中的北向速度误差，φe表示导航坐标系中的东向失准角，φn表示导航坐标系中的北向失准角，φu表示导航坐标系中的方位失准角，表示载体坐标系中右向轴的加速度计随机常值偏置，表示载体坐标系中前向轴的加速度计随机常值偏置，εx表示载体坐标系中右向轴的陀螺随机常值漂移，εy表示载体坐标系中前向轴的陀螺随机常值漂移，εz表示载体坐标系中天向轴的陀螺随机常值漂移；

传递对准系统状态方程如式(2)所示：

其中，x(t)为连续时间系统(12×1)维状态向量，是x(t)的微分形式，g(t)表示连续时间系统的系统噪声驱动矩阵，w(t)表示连续时间系统零均值高斯白噪声；f(t)表示连续时间系统(12×12)维状态转移矩阵，如式(3)所示：

其中，05×7表示(5×7)维的零矩阵，05×5表示(5×5)维的零矩阵，f1(t)如下式所示：

其中，rm表示子午圈曲率半径，rn表示卯酉圈曲率半径，ve表示东向速度，vn表示北向速度，l表示纬度，ωie表示地球旋转角速率，fe表示比力的东向分量，fn表示比力的北向分量，fu表示比力的天向分量；

c(t)如下式所示：

式中，捷联惯性导航系统的姿态矩阵定义为t11表示姿态矩阵的第1行第1列元素，t12表示姿态矩阵的第1行第2列元素，t21表示姿态矩阵的第2行第1列元素，t22表示姿态矩阵的第2行第2列元素，t13表示姿态矩阵的第1行第3列元素，t23表示姿态矩阵的第2行第3列元素，t31表示姿态矩阵的第3行第1列元素，t32表示姿态矩阵的第3行第2列元素，t33表示姿态矩阵的第3行第3列元素；

步骤102)建立传递对准系统量测方程：

传递对准系统量测方程如式(4)所示：

其中，y(t)表示连续时间系统时间t时刻的量测向量；δve表示子惯导和主惯导东向速度误差，δvn表示子惯导和主惯导北向速度误差，δh表示子惯导和主惯导航向角误差，δp表示子惯导和主惯导俯仰角误差，δr表示子惯导和主惯导横滚角误差，表示子惯导的东向速度，表示主惯导的东向速度，表示子惯导的北向速度，表示主惯导的北向速度，表示子惯导的航向角,h^m表示主惯导的航向角,表示子惯导的俯仰角，p^m表示主惯导的俯仰角，表示子惯导的横滚角，r^m表示主惯导的横滚角；

系统的量测方程如式(5)所示：

y(t)＝h(t)x(t)+v(t)式(5)

其中，v(t)表示连续时间系统量测噪声向量，h(t)表示连续时间系统量测矩阵，h(t)如下式所示：

其中，i2×2表示(2×2)维的单位矩阵，02×10表示(2×10)维的零矩阵，03×2表示(3×2)维的零矩阵，03×7表示(3×7)维的零矩阵，h1(t)如下式所示：

步骤103)建立传递对准惯性器件误差补偿模型：

传递对准过程中，陀螺随机常值漂移误差补偿模型如式(6)所示：

其中，表示补偿之后的三轴，即x轴、y轴、z轴，陀螺随机常值漂移，表示三轴随机常值漂移，表示三轴随机慢变漂移，表示三轴随机快变漂移，表示标定的三轴随机常值漂移；

传递对准过程中，加速度计随机常值偏置误差补偿模型如式(7)所示：

其中，表示补偿后的两轴，即x轴、y轴加速度计随机常值偏置，表示两轴加速度计随机常值偏置，表示两轴加速度计随机快变常值偏置，表示标定的两轴加速度计常值偏置。

作为优选例，所述的步骤20)包括：

进行时间更新，所述进行时间更新包括以下过程：

根据式(8)计算状态一步预测值：

其中，表示离散时间系统(12×1)维一步预测状态估计值，φk+1,k表示离散时间系统(12×12)维状态转移矩阵，表示离散时间系统(12×1)维状态估计值；

根据式(9)计算状态估计值：

其中，表示离散时间系统k+1时刻的状态估计值，表示离散时间系统k到k+1时刻的一步预测状态估计值，kk+1表示离散时间系统k+1时刻滤波增益；yk+1表示离散时间系统k+1时刻子惯导和主惯导的量测向量，hk+1表示离散时间系统k+1时刻量测矩阵；

进行量测更新，所述量测更新包括以下过程：

判断滤波器的收敛状态:

选取不等式pk表示离散时间系统的均方误差矩阵；hk表示离散时间系统在k时刻量测矩阵，lk表示(12×12)维矩阵，ξ表示自适应补偿h无穷滤波器的鲁棒因子；

首先判断是否成立，如果不成立，则调整pk值；如果成立，则保持pk值不变；

所述调整pk值的过程为：

计算过程矩阵gk：

式中，diag表示取矩阵的对角线元素组成对角矩阵，符号||表示取矩阵元素的绝对值；

构造过程均方误差矩阵使其满足式(11)：

对式(11)进行变换，得到过程均方误差矩阵如式(12)所示：

构造补偿矩阵使其满足式(13)：

用过程均方误差矩阵和补偿矩阵求取调整之后新的均方误差矩阵如式(14)所示：

其中，σ表示增益系数；

利用新的均方误差矩阵取代pk，从而实现对pk的调整；

进行量测更新：

首先，按照式(15)计算滤波信息λk：

其中，yk表示离散时间系统k时刻子惯导和主惯导的量测向量；表示离散时间系统k-1到k时刻的一步预测状态估计值；

其次，根据式(16)计算构造系数

其中，ζ表示相关系数，且ζ>0，n表示滤波状态的维数；

接着，根据式(17)计算鲁棒因子ξ：

其中，λmax表示取矩阵的最大特征值；

根据式(18)计算初始过程矩阵ks,k+1：

其中，i表示(5×5)维的单位矩阵；

根据式(19)计算中间过程矩阵υk+1：

υk+1＝(i-ks,k+1hk)pk式(19)

根据式(20)计算最终过程矩阵uk+1：

根据式(21)计算k+1时刻的均方差矩阵pk+1：

其中，γk表示离散时间系统系统噪声驱动矩阵，表示矩阵γk的转置；

根据式(22)计算k+1时刻的滤波增益kk+1：

判断当前时刻是否小于预设时间t，如果小于，则返回式(9)，计算下一时刻的状态估计值如果当前时刻等于预设时间，则将最后时刻对应的滤波增益带入式(9)中，得到最后时刻的状态估计值

根据以及式(1)，得到传递对准中子惯导的失准角的估计值；根据所述失准角的估计值对子惯导的姿态矩阵进行一次性修正，完成整个传递对准过程。

有益效果：与现有技术相比，本发明实施例的基于自适应补偿h无穷滤波的传递对准方法，保证了传递对准过程中滤波器的稳定性，同时提高了传递对准精度。

在本实施例的方法中，通过式(10)—(14)中，通过对不等式进行判断，并相应地对均方误差矩阵pk进行自适应调整，可有效抑制滤波发散，提高滤波估计精度。与卡尔曼滤波器和h无穷滤波器相比，自适应补偿h无穷滤波器增加了对输入导航数据的不等式判断环节。当不等式不满足时，自适应调整补偿均方误差矩阵pk，用补偿之后的均方误差矩阵代替pk进行滤波更新，从而有效提高滤波器在复杂动态环境下的滤波精度和稳定性。因此，相比于卡尔曼滤波器和h无穷滤波器，自适应补偿h无穷滤波器更能够适应复杂动态环境和外界强干扰的情况，对异常的导航数据能够做出相应的处理，保证了滤波器的稳定性，同时提高了滤波精度和传递对准精度。

本实施例的方法中，在式(15)—式(17)中，根据外界环境的变化自适应调整鲁棒因子ξ的大小，避免了传统的h无穷滤波器鲁棒因子ξ的大小需要由经验确定且固定不变的缺陷。在自适应补偿h无穷滤波方法中，鲁棒因子ξ的大小与的值成反比，而滤波新息λk的大小反映了外界环境的变化情况。当外界干扰较大时，滤波新息λk的值增大，此时减小鲁棒因子ξ的值，提高自适应补偿h无穷滤波器的鲁棒性。当外界干扰较小时，滤波新息λk的值减小，此时增大鲁棒因子ξ的值，提高自适应补偿h无穷滤波器的估计精度。而传统的h无穷滤波器鲁棒因子ξ的大小是由经验确定的，且在滤波过程中始终保持不变，因此具有一定的局限性，滤波精度也有待提高。

附图说明

图1为本发明实施例的流程图。

图2为本发明试验中，自适应补偿h无穷滤波中鲁棒因子ξ值的曲线图。

图3为本发明试验中，传递对准东向失准角估计值曲线图。

图4为本发明试验中，传递对准北向失准角估计值曲线图。

图5为本发明试验中，传递对准方位失准角估计值曲线图。

图6(a)为本发明试验中，传递对准载体坐标系右向轴陀螺随机常值漂移估计值曲线图。

图6(b)为本发明试验中，传递对准载体坐标系前向轴陀螺随机常值漂移估计值曲线图。

图6(c)为本发明试验中，传递对准载体坐标系天向轴陀螺随机常值漂移估计值曲线图。

图7(a)为本发明试验中，传递对准载体坐标系右向轴加速度计随机常值偏置估计值曲线图。

图7(b)为本发明试验中，传递对准载体坐标系前向轴加速度计随机常值偏置估计值曲线图。

图8为本发明试验中，纯惯性导航位置误差曲线图。

具体实施方式

下面结合附图，对本发明实施例的技术方案进行详细的说明。

如图1所示，本发明实施例的一种基于自适应补偿h无穷滤波的传递对准方法，包括：

步骤10)建立传递对准模型。

步骤20)对步骤10)建立的传递对准模型，进行自适应补偿h无穷滤波，估计失准角，完成传递对准。

作为优选例，所述的步骤10)包括：

步骤101)建立传递对准系统状态方程：

选取12维状态建立系统的状态向量x(t)，系统状态向量x(t)如式(1)所示：

其中，δl表示纬度误差，δλ表示经度误差,δve表示导航坐标系中的东向速度误差，δvn表示导航坐标系中的北向速度误差，φe表示导航坐标系中的东向失准角，φn表示导航坐标系中的北向失准角，φu表示导航坐标系中的方位失准角，表示载体坐标系中右向轴(x轴)的加速度计随机常值偏置，表示载体坐标系中前向轴(y轴)的加速度计随机常值偏置，εx表示载体坐标系中右向轴(x轴)的陀螺随机常值漂移，εy表示载体坐标系中前向轴(y轴)的陀螺随机常值漂移，εz表示载体坐标系中天向轴(z轴)的陀螺随机常值漂移；

传递对准系统状态方程如式(2)所示：

其中，x(t)为连续时间系统(12×1)维状态向量，是x(t)的微分形式，g(t)表示连续时间系统的系统噪声驱动矩阵，w(t)表示连续时间系统零均值高斯白噪声；f(t)表示连续时间系统(12×12)维状态转移矩阵，如式(3)所示：

其中，05×7表示(5×7)维的零矩阵，05×5表示(5×5)维的零矩阵，f1(t)如下式所示：

其中，rm表示子午圈曲率半径，rn表示卯酉圈曲率半径，ve表示东向速度，vn表示北向速度，l表示纬度，ωie表示地球旋转角速率，fe表示比力的东向分量，fn表示比力的北向分量，fu表示比力的天向分量；

c(t)如下式所示：

式中，捷联惯性导航系统的姿态矩阵定义为t11表示姿态矩阵的第1行第1列元素，t12表示姿态矩阵的第1行第2列元素，t21表示姿态矩阵的第2行第1列元素，t22表示姿态矩阵的第2行第2列元素，t13表示姿态矩阵的第1行第3列元素，t23表示姿态矩阵的第2行第3列元素，t31表示姿态矩阵的第3行第1列元素，t32表示姿态矩阵的第3行第2列元素，t33表示姿态矩阵的第3行第3列元素；

步骤102)建立传递对准系统量测方程：

传递对准系统量测方程如式(4)所示：

其中，y(t)表示连续时间系统时间t时刻的量测向量；δve表示子惯导和主惯导东向速度误差，δvn表示子惯导和主惯导北向速度误差，δh表示子惯导和主惯导航向角误差，δp表示子惯导和主惯导俯仰角误差，δr表示子惯导和主惯导横滚角误差，表示子惯导的东向速度，表示主惯导的东向速度，表示子惯导的北向速度，表示主惯导的北向速度，表示子惯导的航向角,h^m表示主惯导的航向角,表示子惯导的俯仰角，p^m表示主惯导的俯仰角，表示子惯导的横滚角，r^m表示主惯导的横滚角；

系统的量测方程如式(5)所示：

y(t)＝h(t)x(t)+v(t)式(5)

其中，v(t)表示连续时间系统量测噪声向量，h(t)表示连续时间系统量测矩阵，h(t)如下式所示：

其中，i2×2表示(2×2)维的单位矩阵，02×10表示(2×10)维的零矩阵，03×2表示(3×2)维的零矩阵，03×7表示(3×7)维的零矩阵，h1(t)如下式所示：

步骤103)建立传递对准惯性器件误差补偿模型：

传递对准过程中，陀螺随机常值漂移误差补偿模型如式(6)所示：

其中，表示补偿之后的三轴，即x轴、y轴、z轴，陀螺随机常值漂移，表示三轴随机常值漂移，表示三轴随机慢变漂移，表示三轴随机快变漂移，表示标定的三轴随机常值漂移；

传递对准过程中，加速度计随机常值偏置误差补偿模型如式(7)所示：

其中，表示补偿后的两轴，即x轴、y轴加速度计随机常值偏置，表示两轴加速度计随机常值偏置，表示两轴加速度计随机快变常值偏置，表示标定的两轴加速度计常值偏置。

作为优选例，所述的步骤20)包括：

步骤201)进行时间更新，所述进行时间更新包括以下过程：

根据式(8)计算状态一步预测值：

其中，表示离散时间系统(12×1)维一步预测状态估计值，φk+1,k表示离散时间系统(12×12)维状态转移矩阵，表示离散时间系统(12×1)维状态估计值；

根据式(9)计算状态估计值：

其中，表示离散时间系统k+1时刻的状态估计值，表示离散时间系统k到k+1时刻的一步预测状态估计值，kk+1表示离散时间系统k+1时刻滤波增益；yk+1表示离散时间系统k+1时刻子惯导和主惯导的量测向量，hk+1表示离散时间系统k+1时刻量测矩阵；

步骤202)进行量测更新，所述量测更新包括以下过程：

判断滤波器的收敛状态:

选取不等式pk表示离散时间系统的均方误差矩阵；hk表示离散时间系统在k时刻量测矩阵；本实施例中，lk表示(12×12)维单位矩阵；ξ表示自适应补偿h无穷滤波器的鲁棒因子；

首先判断是否成立，如果不成立，则调整pk值；如果成立，则保持pk值不变；

所述调整pk值的过程为：

计算过程矩阵gk：

式中，diag表示取矩阵的对角线元素组成对角矩阵，符号||表示取矩阵元素的绝对值；

构造过程均方误差矩阵使其满足式(11)：

对式(11)进行变换，得到过程均方误差矩阵如式(12)所示：

构造补偿矩阵使其满足式(13)：

用过程均方误差矩阵和补偿矩阵求取调整之后新的均方误差矩阵如式(14)所示：

其中，σ表示增益系数；

利用新的均方误差矩阵取代pk，从而实现对pk的调整。

调整pk的目的是抑制滤波发散，保证外界干扰较强或者数据有异常时滤波器的稳定性，同时提高复杂动态环境下的滤波精度。

步骤203)：进行量测更新：

首先，按照式(15)计算滤波信息λk：

其中，yk表示离散时间系统k时刻子惯导和主惯导的量测向量；表示离散时间系统k-1到k时刻的一步预测状态估计值；

其次，根据式(16)计算构造系数

其中，ζ表示相关系数，且ζ>0，n表示滤波状态的维数。本实施例中，n＝12。

接着，根据式(17)计算鲁棒因子ξ：

其中，λmax表示取矩阵的最大特征值；

根据式(18)计算初始过程矩阵ks,k+1：

其中，i表示(5×5)维的单位矩阵；

根据式(19)计算中间过程矩阵υk+1：

υk+1＝(i-ks,k+1hk)pk式(19)

根据式(20)计算最终过程矩阵uk+1：

根据式(21)计算k+1时刻的均方差矩阵pk+1：

其中，γk表示离散时间系统系统噪声驱动矩阵，表示矩阵γk的转置；

根据式(22)计算k+1时刻的滤波增益kk+1：

判断当前时刻是否小于预设时间t，如果小于，则返回式(9)，计算下一时刻的状态估计值如果当前时刻等于预设时间，则将最后时刻对应的滤波增益带入式(9)中，得到最后时刻的状态估计值

根据以及式(1)，得到传递对准中子惯导的失准角的估计值；根据所述失准角的估计值对子惯导的姿态矩阵进行一次性修正，完成整个传递对准过程。

上述实施例中，x(t)是连续时间系统的状态向量，是离散系统的状态向量。两者区别仅在于一个是连续时间系统的表达形式，一个是离散时间系统的表达形式，含义其实是一样的。

在上述实施例的方法中，在式(4)中，基于自适应补偿h无穷滤波的传递对准方法的传递对准系统量测方程采用了速度+姿态的匹配方法，相比于传统的速度匹配方法，增加了主惯导和子惯导的姿态角的差值作为量测量，可有效减小传递对准时间，同时提高传递对准精度。

在上述实施例的方法中，在式(6)和式(7)中，基于自适应补偿h无穷滤波的传递对准方法在传递对准过程中对子惯导的陀螺随机常值漂移和加速度计随机常值偏置进行在线标定，并在对准结束后用标定值对子惯导的陀螺随机常值漂移和加速度计随机常值偏置进行补偿。与不进行标定和补偿的方法相比，该方法在传递对准过程中同时对惯性器件误差进行了在线标定和补偿，有效提高了器件的精度。

为验证本发明的方法所具有的优良性能，共进行了七组机载传递对准试验，并在传递对准结束后，通过纯惯性导航验证传递对准效果。试验参数设置如下:

飞机所处的地理经度λ＝119°e，纬度l＝32°n；主、子惯导更新频率均为200hz；捷联算法更新周期5ms；子惯导三轴陀螺随机常值漂移为1.2°/h；子惯导三轴加速度计随机常值偏置为0.1mg；传递对准时间234s；纯惯性导航时间600s。

滤波器初始值设置如下：初始值设置为均方误差矩阵初始值设置为p0＝diag{10m,10m,(0.7m/s)²，(0.7m/s)²，(0.8°)²，(0.8°)²，(1.5°)²，(0.1×10^-3g)²，(0.1×10^-3g)²，(1.2°/h)²，(1.2°/h)²，(1.2°/h)²}，系统噪声驱动矩阵初值设置为γ0＝diag{0,0,0.05′10^-3g,0.05′10^-3g,0.6°/h,0.6°/h,0.6°/h,0,0,0,0,0}。

按照本实施例的方法，通过式(24)实时计算鲁棒因子ξ的值，如图2所示。

按照本实施例的方法，在传递对准过程中，通过自适应补偿h无穷滤波对状态估计值进行实时迭代计算，得到式(1)中东向失准角的估计值，如图3所示。

按照本实施例的方法，在传递对准过程中，通过自适应补偿h无穷滤波对状态估计值进行实时迭代计算，得到式(1)中北向失准角的估计值，如图4所示。

按照本实施例的方法，在传递对准过程中，通过自适应补偿h无穷滤波对状态估计值进行实时迭代计算，得到式(1)中方位失准角的估计值，如图5所示。

按照本实施例的方法，在传递对准过程中，通过自适应补偿h无穷滤波对状态估计值进行实时迭代计算，得到式(1)中载体坐标系右向轴，前向轴，天向轴陀螺随机常值漂移的估计值，可以得到载体坐标系右向轴，前向轴，天向轴陀螺随机常值漂移的估计值曲线图，如图6(a)至6(c)所示。

按照本实施例的方法，在传递对准过程中，通过自适应补偿h无穷滤波对状态估计值进行实时迭代计算，得到式(1)中载体坐标系右向轴，前向轴加速度计随机常值偏置的估计值，可以得到载体坐标系右向轴，前向轴加速度计随机常值偏置的估计值曲线图，如图7(a)和图7(b)所示。

为验证本发明的传递对准方法的精度，进行了600s的纯惯性导航试验，由子惯导实时解算实际位置，并与理论位置相减，可得到纯惯性导航位置误差曲线图，如图8所示。从图8可以看出：经过600s的纯惯性导航，子惯导的位置误差在350m以内，位置误差控制在较小的范围内。这验证了基于自适应补偿h无穷滤波的传递对准方法的有效性。

以上显示和描述了本发明的基本原理、主要特征和优点。本领域的技术人员应该了解，本发明不受上述具体实施例的限制，上述具体实施例和说明书中的描述只是为了进一步说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护的范围由权利要求书及其等效物界定。