[发明专利]一种无需初始对准的任意姿态MEMS组合定姿方法

权利要求书

1.一种无需初始对准的任意姿态MEMS组合定姿方法，其特征在于步骤如下：

1)定义修正Rodrigues参数；

2)建立基于修正Rodrigues参数的非线性姿态误差状态方程；

3)建立磁强计的非线性观测方程；

4)对非线性姿态误差状态方程和磁强计的非线性观测方程进行线性化处理，得到状态误差的线性化模型和磁强计的线性化观测模型；

5)利用得到的非线性姿态误差状态误差方程和磁强计的非线性观测方程，结合状态误差的线性化状态模型与磁强计的线性化观测模型，利用扩展Kalman滤波方法得到修正Rodrigues参数的最优估计，作为姿态结果供制导控制使用，实现任意姿态的MEMS和磁强计组合定姿；

所述步骤1)中修正Rodrigues参数的具体形式为：其中为旋转轴的单位向量，θ为转过的角度，上标^T表示向量或矩阵的转置；

所述步骤2)中建立基于修正Rodrigues参数的非线性姿态误差状态方程的具体形式为：

其中δσ＝[δσ₁ δσ₂ δσ₃]^T为修正Rodrigues参数的误差，为MEMS陀螺测量误差，为MEMS惯组本体坐标系相对于参考坐标系的旋转角速度在MEMS惯组本体系下的投影，为MEMS惯组输出的测量值，符号||·||表示向量的模，f(δσ)表示修正的Rodrigues参数状态误差时间变化率与δσ的函数关系；

所述步骤3)中建立磁强计的非线性观测方程的具体形式为：

其中磁强计观测误差为通过地磁场模型、姿态四元数计算得到的本体四元数，为磁强计测量值，g(δσ)表示磁强计测量误差中与δσ相关的函数关系。

2.根据权利要求1所述的一种无需初始对准的任意姿态MEMS组合定姿方法，其特征在于：所述步骤4)中对非线性姿态误差状态方程和磁强计的非线性观测方程进行线性化处理，得到状态误差的线性化模型的具体形式为：

其中表示f(δσ)对状态误差的偏导数，G表示状态误差时间变化率对MEMS陀螺测量误差的偏导数。

3.根据权利要求2所述的一种无需初始对准的任意姿态MEMS组合定姿方法，其特征在于：所述f(δσ)对状态误差的偏导数的具体形式为：

I_3×3为3×3维的单位矩阵。

4.根据权利要求3所述的一种无需初始对准的任意姿态MEMS组合定姿方法，其特征在于：表示状态误差时间变化率对MEMS陀螺测量误差的偏导数G的具体形式为：

5.根据权利要求4所述的一种无需初始对准的任意姿态MEMS组合定姿方法，其特征在于：所述步骤4)中对非线性姿态误差状态方程和磁强计的观测方程进行线性化处理，得到磁强计的线性化观测模型的具体形式为：

其中表示磁强计线性化观测模型中g(δσ)对δσ的偏导数。

6.根据权利要求5所述的一种无需初始对准的任意姿态MEMS组合定姿方法，其特征在于：所述磁强计误差模型中g(δσ)对δσ的偏导数的具体公式如下：

7.根据权利要求1-6任一所述的一种无需初始对准的任意姿态MEMS组合定姿方法，其特征在于：所述步骤5)中利用扩展Kalman滤波方法得到修正Rodrigues参数的最优估计的具体过程为：利用得到的非线性姿态误差状态误差方程和磁强计的非线性观测方程，结合状态误差的线性化状态模型与磁强计的线性化观测模型，利用扩展Kalman滤波方法，递推得到修正Rodrigues参数的最优估计，作为姿态结果供制导控制使用，实现任意姿态的MEMS和磁强计组合定姿。