[发明专利]一种基于扩展卡尔曼滤波的云台姿态估计方法

说明书

本发明提供一种基于扩展卡尔曼滤波的云台姿态估计方法，该方法采用四元数来表示物体当前的姿态，系统状态量包含四元数与角度增量的偏移误差，使用加速度计和磁力计修正角度增量的偏移误差，使得姿态估计更加精确，且将加速计修正与磁力计修正分为两阶段实行，使得加速度计修正与磁力计修正互不干扰，提高姿态估计精确度；在加速度计修正中，把修正量中的四元数第三矢量置为零，在磁力计修正中，把修正量中的四元数第一矢量和第二矢量置为零。采用本发明能够获得更为精准的姿态估计信息。

技术领域

本发明涉及卡尔曼滤波技术，具体涉及一种基于四元数扩展卡尔曼滤波的云台姿态估计算法。

背景技术

近年来，随着手持云台等智能设备的广泛应用，人们对其的稳定性和精确性都提出了更高的要求，要使这些设备能够更加稳定工作，获取更为精准的姿态估计信息尤为重要。在大多数姿态估计算法中，都是利用陀螺仪更新姿态信息，利用加速度计和磁力计对姿态信息进行修正。目前应用较广的姿态估计算法有卡尔曼滤波算法和互补滤波算法，这两种算法均能减少高斯噪声等对姿态估计的影响，也能减少由于陀螺仪漂移所造成的累计误差。

但是，由于加速度计和磁力计都是极易受到外界干扰的传感器，采用互补滤波算法精度往往有所欠缺，采用传统卡尔曼滤波算法的效果也差强人意。因此，衍生出了扩展卡尔曼滤波算法，扩展卡尔曼滤波算法相较于传统卡尔曼滤波算法更适用于姿态估计。

发明内容

为了实时获取更为精准的姿态估计信息，本发明提出了一种基于扩展卡尔曼滤波的云台姿态估计方法，用于估计出非线性状态模型下的云台姿态信息。

为达到上述目的，本发明提供的基于扩展卡尔曼滤波的云台姿态估计方法,具体包括如下步骤：

1)采用航向姿态参考系统(AHRS)来给云台控制器提供较为精确的姿态信息，姿态信息用欧拉角进行表示，欧拉角包括俯仰角、横滚角和偏航角，采用四元数q表示欧拉角；

2)初始化系统状态，其中系统状态量如下式所示：

式(1)中，表示系统状态量，q表示四元数；q＝[q₀ q₁ q₂ q₃]^T；Δθ_b表示角度增量的偏移误差；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

3)读取陀螺仪数据，对旋转四元数进行预测；

式(2)中，Δθ_m表示陀螺仪实际测得的角度增量；Δθ_b表示角度增量的偏移误差；Δθ_n表示角增量噪声；q_k表示k时刻的四元数；q_k+1表示k+1时刻的四元数；k表示时刻；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

4)更新先验协方差矩阵P_k+1/k；

5)使用加速度计对系统状态量进行修正；更新加速度计修改后的后验协方差矩阵P_{k+1/k+1_accel}；

6)使用磁力计对系统状态量进行修正；更新磁力计修正后的后验协方差矩阵P_{k+1/k+1_mag}。

所述导航坐标系定为东-北-天。

其中，所述步骤4)具体为：

4-1)计算k时刻至k+1时刻的一步转移阵F；

式(3)中由式(2)q_k+1对q_k求导得到，由式(2)q_k+1对求导得到；