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

权利要求书

1.一种基于扩展卡尔曼滤波的云台姿态估计方法,其特征在于，包括如下步骤：

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；具体包括：

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

式(3)中由式(2)q_k+1对q_k求导得到，由式(2)q_k+1对求导得到；k表示时刻；

4-2)计算系统噪声驱动阵Γ；

式(4)中由式(2)q_k+1对求导得到；k表示时刻；

4-3)计算系统噪声方差阵Q；

式(5)中Δθ_n表示角增量噪声；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

4-4)计算过程噪声矩阵；

式(6)中为陀螺仪偏差的过程噪声协方差；

4-5)得到先验协方差矩阵P_k+1/k；

P_k+1/k＝F·P_k/k·F^T+Γ·Q·Γ^T+N_process (7)

式(7)中，F^T表示F矩阵的转置矩阵；Γ^T表示Γ矩阵的转置矩阵；k表示时刻；

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

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

2.如权利要求1所述基于扩展卡尔曼滤波的云台姿态估计方法,其特征在于，导航坐标系定为东-北-天。