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

说明书

本发明一种无需初始对准的任意姿态MEMS组合定姿方法，步骤如下：1)定义修正Rodrigues参数；2)建立基于修正Rodrigues参数的非线性姿态误差状态方程；3)建立磁强计的非线性观测方程；4)对非线性姿态误差状态方程和磁强计的观测方程进行线性化处理，得到状态误差的线性化状态误差模型和磁强计的线性化观测模型；5)利用得到的非线性状态误差方程和观测方程，结合线性化状态模型与观测模型，利用扩展Kalman滤波方法得到修正Rodrigues参数的最优估计，作为姿态结果供制导控制使用，实现任意姿态的MEMS和磁强计组合定姿。

技术领域

本发明属于导航制导领域，涉及一种无需初始对准的任意姿态MEMS组合定姿方法。

背景技术

以MEMS惯性器件作为主导航设备构建导航系统通过积分计算来实现定姿定位，因而需要在发射前进行初始对准。一般常用的初始对准方案需要地面建立基准点，通过地面光电经纬仪进行光瞄，获取初始导航系统初始姿态信息。需配套相应的地面设备、制作操作流程，对准时间长，作战准备复杂。初始对准耗时是影响发射准备时间的重要因素，而缩短发射准备时间是提高导弹武器系统快速反应能力，提高作战效能的重要手段。

发明内容

本发明解决的技术问题是：针对初始对准耗时降低武器作战效能的问题，提出一种无需初始对准的任意姿态MEMS组合定姿技术，通过磁强计来实现三轴姿态确定，无需地面配备发射准备条件，同时大大缩短发射准备时间，有效地解决了无依托发射中MEMS惯组初始对准问题。

本发明的技术方案是：一种无需初始对准的任意姿态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(δσ)表示磁强计测量误差中与状态误差δσ相关的函数关系。

所述步骤4)中对非线性姿态误差状态方程和磁强计的观测方程进行线性化处理，得到状态误差的线性化模型的具体形式为：

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