[发明专利]基于分布式循环结构的飞行器自主姿态估计方法

权利要求书

1.基于分布式循环结构的飞行器自主姿态估计方法，其特征在于，包括以下步骤：

步骤一：利用离散四元数动力学方程建立飞行器姿态估计系统状态方程和三轴磁强计测量模型建立姿态估计系统观测方程，从而完整构建飞行器姿态估计系统，并求解该姿态估计系统，从而得到当前时刻的姿态四元数、姿态转换矩阵、三轴姿态，利用当前时刻姿态矩阵将当地地磁场矢量理论值转化到载体坐标系下，将转化到载体坐标系下的当地地磁场矢量值与校正后的三轴磁强计测量值做差，得到地磁场残差向量；地磁场残差向量Δy_k为当地地磁场向量在机体坐标系下的理论值与经过误差校正的三轴磁强计测量值之差；其中，为滤波后的四元数最优估计值，H_k为地理坐标系下磁场向量值，A(·)为从地理坐标系到载体坐标系的姿态转换矩阵，B_k,m为磁强计在k时刻的测量值，d_k为零偏，s_k为尺度因数；

步骤二：利用加入测量噪声的残差向量构建三轴磁强计校正系统观测方程，以及由尺度因素误差和零偏两个误差参数作为系统状态构建的三轴磁强计校正系统状态方程，由此构建完整的三轴磁强计校正系统，并利用UKF滤波器估计该系统当前时刻的状态，也即误差参数；

步骤三：利用估计出的当前时刻的误差参数校正三轴磁强计测量值，并将校正好的测量值代入飞行器姿态估计系统，重新估计三轴姿态；

步骤二中三轴磁强计校正系统观测方程构建方法为：

令n为一个循环中积累的量测值数量，那么将观测方程写为

z_i＝Δy_i+r_i,i＝1,…,n

其中，zi是在一个循环内积累的第i个时刻的误差，Δyi为地磁场残差向量，ri是相应的噪声；

步骤二中三轴磁强计校正系统状态方程构建方法为：

其中，s_i为尺度因素，d_i为零偏，b_i是一个过程噪声项，用来表示尺度因素和零偏的变化量；

尺度因数s_i和零偏d_i这两个估计量用校正过的地面测试数据初始化，并且在载体运行阶段通过传感器校正过程进行修正，最终得到当时时刻的最优估计值

对三轴磁强计的测量值进行校正，方法如下：

得到校正后的三轴磁强计测量值B′_k,m，将该值代入姿态估计系统循环，估计飞行器姿态。

2.根据权利要求1所述的基于分布式循环结构的飞行器自主姿态估计方法，其特征在于，步骤一中姿态估计系统观测方程构建方法为：

其中，为当前k时刻的姿态四元数一步预测值，A(·)为从地理坐标系到载体坐标系的姿态转换矩阵，B_k为在载体坐标系下的三轴磁强计测量值，H_k为地理坐标系下磁场向量值，v_k为测量噪声。

3.根据权利要求1所述的基于分布式循环结构的飞行器自主姿态估计方法，其特征在于，步骤一中飞行器姿态估计系统求解方法为：

将离散的飞行器姿态估计系统代入滚动时域估计模型，采用高斯-牛顿迭代法并结合MEMS速率陀螺和校正好的三轴磁强计测量值求解该滚动时域估计模型，从而得到当前时刻的姿态四元数、姿态转换矩阵、三轴姿态。