CN110849364B - 基于动中通的自适应卡尔曼姿态估计方法 - Google Patents

Abstract

本发明公开了供基于动中通的自适应卡尔曼姿态估计方法，根据当前系统状态协方差矩阵和状态向量的观测方程的雅克比矩阵以及观测向量噪声协方差矩阵，计算出卡尔曼增益矩阵的最优估计。本发明采用动中通卫星通信系统提供的水平航向角作为系统观测的方位通道，克服了现有技术在低速和强磁干扰环境下方位角无法观测的困难，提供稳定的方位角度；克服了现有技术姿态估计容易受外界因素干扰的问题，提高姿态估计精度，满足动中通卫星通信系统波速指向需求。

Description

基于动中通的自适应卡尔曼姿态估计方法

技术领域

本发明属于组合姿态测量、通信领域，尤其涉及基于动中通的自适应卡尔曼姿态估计方法。

背景技术

目前应用于动中通卫星通信系统中的姿态测量主要使用惯性器件加速度计和陀螺仪，使用导航信息或者磁力计获得航向信息；姿态估计算法有传感器直接融合算法和互补滤波算法等。

现有技术存在以下缺点：

(1)使用导航信息获得航向信息，单天线的接收机需要有一定的速度，才能提供准确的航向信息，若需在载体静止时提供准确的航向信息，需使用双天线接收机等设备，增加了成本，同时对双天线的安装间隔(基线)有距离要求，一般满足要求的基线在0.6米以上，对小型的动中通无法部署；

(2)通过磁力计获得航向信息，磁力计在使用过程中容易受到外界环境干扰，在使用时需要经常校准，而动中通天线上使用的电机就是一个很强的干扰源，航向精度难以保证；

(3)采用多传感器直接融合姿态估计算法，多传感器直接融合姿态估计算法易受载体机动加速度和侧滑角等外界因素干扰，导致姿态估计误差较大；

(4)采用互补滤波姿态估计算法，但互补滤波算法属于常增益算法，抗干扰能力差，在复杂的环境中容易受外界干扰导致姿态误差大且难以收敛等问题。

发明内容

本发明的目的就在于为了解决上述问题而提供基于动中通的自适应卡尔曼姿态估计方法，包括如下步骤：

建立非线性离散系统：x(k)∈R表示k时刻系统的状态向量，q₀(k)、q₁(k)、q₂(k)、q₃(k)为姿态四元数；bx(k)、by(k)、bz(k)分别为横滚轴、俯仰轴和方位轴的陀螺仪随机漂移向量；ax(k)，ay(k)，az(k)表示加速度计测得k时刻载体的三轴加速度，

是天线测得惯导当前的航向角；z(k)∈R表示k时刻系统的观测矢量，w(k-1)为系统迭代过程中产生的噪声，v(k)是系统观测噪声；

系统状态向量：

x(k)＝[q₀(k)q₁(k)q₂(k)q₃(k)bx(k)by(k)bz(k)]^T；

系统观测向量：

非线性离散系统模型：

基于动中通的自适应卡尔曼姿态估计方法通过以下步骤来实现：

系统状态向量转移：系统状态向量从k-1时刻迭代到k时刻，得到k时刻的系统状态向量

即k时刻的估计值，系统状态向量转移关系由系统状态向量转移方程决定；其中将上一时刻的系统状态向量x(k-1)和系统状态协方差矩阵P(k-1)作为系统的初始值；

设I表示单位矩阵，T表示积分周期，Ω＝[0ω_xω_yω_z]，ω_x、ω_y、ω_z表示陀螺仪测量载体三轴角速度；则系统的状态转移方程：

由k-1时刻的系统状态以及三轴角速度根据状态转移函数关系f(x(k-1),k-1)得到k时刻系统状态的估计值

f(x(k-1),k-1)是非线性函数，对其求导即计算其雅克比矩阵；

同时k-1时刻的系统状态协方差矩阵P(k-1)根据系统状态向量转移方程以及系统状态转移过程噪声协方差矩阵Q(k)转移到k时刻：

P(k，k-1)＝φ(k，k-1)×P(k-1)×φ^T(k，k-1)+Q(k)；

设

表示四元数迭代过程中产生的噪声的方差，

表示陀螺仪噪声的方差，I_4x4表示4阶单位矩阵，I_3x3表示3阶单位矩阵；系统状态转移过程噪声协方差矩阵Q(k)：

系统完成预测过程，如果没有观察过程发生，则系统根据上述的转移方式在时间序列一直传播下去；系统采用加速度计测得的三轴加速度和天线测得的航向角组成观测向量，观测向量与系统状态向量之间的关系由观测向量状态转移方程h(x(k),k)决定；

状态向量的观测方程为：

其中g表示当地的重力加速度；该状态向量的观测方程用于反映k时刻系统状态向量的估计值

与k时刻的观测向量z(k)的变换关系；系统状态向量与观测向量关系属于非线性关系对函数

进行求导，即求其雅克比矩阵；

设

表示加速度计测量产生噪声的方差，

表示天线测量航向角的方差，I_3×3表示3阶单位矩阵，I_1×1表示1阶单位矩阵；观测向量噪声协方差矩阵用于反映观测向量的准确度；

观测向量噪声协方差矩阵：

a(k)表示k时刻加速度计的模长，C₀为设定的加速度计权重因子，E(k)为天线方位跟踪误差，由天线直接提供，C₁为设定的天线权重因子；

观测向量噪声协方差自适应：

当载体线性加速度干扰变大或者跟踪误差变大时，影响观测向量噪声协方差；在卡尔曼滤波过程中，一方面利用测量值不断修正期望值，另一方面也对系统模型参数或噪声统计参数进行调整；

根据当前系统状态协方差矩阵P(k,k-1)和状态向量的观测方程的雅克比矩阵H(k)以及观测向量噪声协方差矩阵R(k)计算卡尔曼增益矩阵，卡尔曼增益矩阵用于反映观测向量对系统状态向量的修正权重；

卡尔曼增益矩阵K(k)：

K(k)＝P(k,k-1)×H^T(k)×[H(k)×P(k,k-1)×H^T(k)+R(k)]^-1；

通过卡尔曼增益矩阵K(k)，k时刻观测向量对系统状态向量进行修正得到k时刻的最优估计：

修正系统状态协方差矩阵，更新为：

P(k)＝[I-K(k)×H(k)]×P(k，k-I)×[I-K(k)×H(k)]^T+K(k)×H(k)×K^T(k)；

其中I为单位矩阵，当前最优估计x₁(k)和状态协方差矩阵P(k)作为下一时刻的初始值迭代。

本发明的有益效果在于：本发明采用动中通卫星通信系统提供的水平航向角作为系统观测的方位通道，克服了现有技术在低速和强磁干扰环境下方位角无法观测的困难，提供稳定的方位角度；克服了现有技术姿态估计容易受外界因素干扰的问题，提高姿态估计精度，满足动中通卫星通信系统波速指向需求。

附图说明

图1是本发明的流程图。

具体实施方式

下面结合附图对本发明作进一步说明：

如附图1所示，本发明基于动中通的自适应卡尔曼姿态估计方法，包括如下步骤：

建立非线性离散系统：x(k)∈R表示k时刻系统的状态向量，q₀(k)、q₁(k)、q₂(k)、q₃(k)为姿态四元数；bx(k)、by(k)、bz(k)分别为横滚轴、俯仰轴和方位轴的陀螺仪随机漂移向量；ax(k)，ay(k)，az(k)表示加速度计测得k时刻载体的三轴加速度，

是天线测得惯导当前的航向角；z(k)∈R表示k时刻系统的观测矢量，w(k-1)为系统迭代过程中产生的噪声，v(k)是系统观测噪声；

系统状态向量：

x(k)＝[q₀(k)q₁(k)q₂(k)q₃(k)bx(k)by(k)bz(k)]^T；

系统观测向量：

非线性离散系统模型：

系统状态向量转移：系统状态向量从k-1时刻迭代到k时刻的过程叫做系统状态向量转移，得到k时刻的系统状态向量

称k时刻的估计值，系统状态向量转移关系由系统状态向量转移方程决定；其中需要将上一时刻的系统状态向量x(k-1)和系统状态协方差矩阵P(k-1)当做系统的初始值；

系统的状态转移方程：

其中I表示单位矩阵，T表示积分周期，Ω＝[0ω_xω_yω_z]，ω_x、ω_y、ω_z表示陀螺仪测量载体三轴角速度；由k-1时刻的系统状态以及三轴角速度根据状态转移函数关系f(x(k-1),k-1)得到k时刻系统状态的估计值

由于f(x(k-1),k-1)是非线性函数，需要对其求导及计算其雅克比矩阵；

计算函数f(x(k-1),k-1)的雅克比矩阵：

同时k-1时刻的系统状态协方差矩阵P(k-1)根据系统状态向量转移方程以及系统状态转移过程噪声协方差矩阵Q(k)转移到k时刻：

P(k，k-1)＝φ(k，k-1)×P(k-1)×φ^T(k，k-1)+Q(k)；

系统状态转移过程噪声协方差矩阵Q(k)：

其中

表示四元数迭代过程中产生的噪声的方差，

表示陀螺仪噪声的方差，I_4x4表示4阶单位矩阵，I_3x3表示3阶单位矩阵；

至此系统完成预测的过程，如果没有观察过程发生，则系统根据上述的转移方式在时间序列一直传播下去；系统采用加速度计测得的三轴加速度和天线测得的航向角组成了观测向量，观测向量与系统状态向量之间的关系由观测向量状态转移方程h(x(k),k)决定；

状态向量的观测方程为：

其中g表示当地的重力加速度；该方程反映了k时刻系统状态向量的估计值

与k时刻的观测向量z(k)的变换关系；由于系统状态向量与观测向量关系属于非线性关系，所以需要对函数

进行求导，即求其雅克比矩阵；

设

表示加速度计测量产生噪声的方差，

表示天线测量航向角的方差，I_3×3表示3阶单位矩阵，I_1×1表示1阶单位矩阵；观测向量噪声协方差矩阵用于反映观测向量的准确度；

观测向量噪声协方差矩阵：

观测向量噪声协方差自适应：

a(k)表示k时刻加速度计的模长，C₀为设定的加速度计权重因子，E(k)为天线方位跟踪误差，由天线直接提供，C₁为设定的天线权重因子；当载体线性加速度干扰变大或者跟踪误差变大时，相应的也影响观测向量噪声协方差；在卡尔曼滤波过程中，一方面利用测量值不断修正期望值，另一方面如果同时也对系统模型参数或噪声统计参数进行调整；

根据当前系统状态协方差矩阵P(k,k-1)和状态向量的观测方程的雅克比矩阵H(k)以及观测向量噪声协方差矩阵R(k)计算卡尔曼增益矩阵，卡尔曼增益矩阵用于反映观测向量对系统状态向量的修正权重；

卡尔曼增益矩阵K(k)：

K(k)＝P(k,k-1)×H^T(k)×[H(k)×P(k,k-1)×H^T(k)+R(k)]^-1；

通过卡尔曼增益矩阵K(k)，k时刻观测向量对系统状态向量进行修正得到k时刻的最优估计：

相应的系统状态协方差矩阵也得到修正，更新为：

P(k)＝[I-K(k)×H(k)]×P(k，k-I)×[I-K(k)×H(k)]^T+K(k)×H(k)×K^T(k)；

其中I为单位矩阵，当前最优估计x₁(k)和对应的状态协方差矩阵P(k)将作为下一时刻的初始值,系统便可以按照上述方案一直迭代下去。

本发明采用动中通卫星通信系统提供的水平航向角作为系统观测的方位通道，克服了现有技术在低速和强磁干扰环境下方位角无法观测的困难，提供稳定的方位角度；克服了现有技术姿态估计容易受外界因素干扰的问题，提高姿态估计精度，满足动中通卫星通信系统波速指向需求。

本发明的技术方案不限于上述具体实施例的限制，凡是根据本发明的技术方案做出的技术变形，均落入本发明的保护范围之内。

Claims (1)

1.基于动中通的自适应卡尔曼姿态估计方法，其特征在于，包括如下步骤：

建立非线性离散系统：x(k)∈R表示k时刻系统的状态向量，q₀(k)、q₁(k)、q₂(k)、q₃(k)为姿态四元数；bx(k)、by(k)、bz(k)分别为横滚轴、俯仰轴和方位轴的陀螺仪随机漂移向量；ax(k)，ay(k)，az(k)表示加速度计测得k时刻载体的三轴加速度，

是天线测得惯导当前的航向角；z(k)∈R表示k时刻系统的观测矢量，w(k-1)为系统迭代过程中产生的噪声，v(k)是系统观测噪声；

系统状态向量：

x(k)＝[q₀(k) q₁(k) q₂(k) q₃(k) bx(k) by(k) bz(k)]^T；

系统观测向量：

非线性离散系统模型：

基于动中通的自适应卡尔曼姿态估计方法通过以下步骤来实现：

系统状态向量转移：系统状态向量从k-1时刻迭代到k时刻，得到k时刻的系统状态向量

即k时刻的估计值，系统状态向量转移关系由系统状态向量转移方程决定；其中将上一时刻的系统状态向量x(k-1)和系统状态协方差矩阵P(k-1)作为系统的初始值；

设I表示单位矩阵，T表示积分周期，Ω＝[0 ω_x ω_y ω_z]，ω_x、ω_y、ω_z表示陀螺仪测量载体三轴角速度；则系统的状态转移方程：

由k-1时刻的系统状态以及三轴角速度根据状态转移函数关系f(x(k-1),k-1)得到k时刻系统状态的估计值

f(x(k-1),k-1)是非线性函数，对其求导即计算其雅克比矩阵；

同时k-1时刻的系统状态协方差矩阵P(k-1)根据系统状态向量转移方程以及系统状态转移过程噪声协方差矩阵Q(k)转移到k时刻：

P(k，k-1)＝φ(k，k-1)×P(k-1)×φ^T(k，k-1)+Q(k)；

设

表示四元数迭代过程中产生的噪声的方差，

表示陀螺仪噪声的方差，I_4x4表示4阶单位矩阵，I_3x3表示3阶单位矩阵；系统状态转移过程噪声协方差矩阵Q(k)：

系统完成预测过程，如果没有观察过程发生，则系统根据上述的转移方式在时间序列一直传播下去；系统采用加速度计测得的三轴加速度和天线测得的航向角组成观测向量，观测向量与系统状态向量之间的关系由观测向量状态转移方程h(x(k),k)决定；

状态向量的观测方程为：

其中g表示当地的重力加速度；该状态向量的观测方程用于反映k时刻系统状态向量的估计值

与k时刻的观测向量z(k)的变换关系；系统状态向量与观测向量关系属于非线性关系，需要对函数

进行求导，即求其雅克比矩阵；

设

表示加速度计测量产生噪声的方差，

表示天线测量航向角的方差，I_3×3表示3阶单位矩阵，I_1×1表示1阶单位矩阵；观测向量噪声协方差矩阵用于反映观测向量的准确度；

观测向量噪声协方差矩阵：

a(k)表示k时刻加速度计的模长，C₀为设定的加速度计权重因子，E(k)为天线方位跟踪误差，由天线直接提供，C₁为设定的天线权重因子；

观测向量噪声协方差自适应：

当载体线性加速度干扰变大或者跟踪误差变大时，影响观测向量噪声协方差；在卡尔曼滤波过程中，一方面利用测量值不断修正期望值，另一方面也对系统模型参数或噪声统计参数进行调整；

根据当前系统状态协方差矩阵P(k,k-1)和状态向量的观测方程的雅克比矩阵H(k)以及观测向量噪声协方差矩阵R(k)计算卡尔曼增益矩阵，卡尔曼增益矩阵用于反映观测向量对系统状态向量的修正权重；

卡尔曼增益矩阵K(k)：

K(k)＝P(k,k-1)×H^T(k)×[H(k)×P(k,k-1)×H^T(k)+R(k)]^-1；

通过卡尔曼增益矩阵K(k)，k时刻观测向量对系统状态向量进行修正得到k时刻的最优估计：

修正系统状态协方差矩阵，更新为：

P(k)＝[I-K(k)×H(k)]×P(k，k-I)×[I-K(k)×H(k)]^T+K(k)×H(k)×K^T(k)；

其中I为单位矩阵，当前最优估计x₁(k)和状态协方差矩阵P(k)作为下一时刻的初始值迭代。

Images