一种无需初始对准的任意姿态MEMS组合定姿方法与流程

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

背景技术：

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

技术实现要素：

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

本发明的技术方案是：一种无需初始对准的任意姿态mems组合定姿方法，其特征在于步骤如下：

1)定义修正rodrigues参数；

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

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

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

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

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

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

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

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

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

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

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

所述状态误差时间变化率f(δσ)对状态误差的偏导数的具体形式为：

所述表示状态误差时间变化率对mems陀螺测量误差的偏导数g的具体形式为：

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

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

其中为磁强计观测模型对状态误差δσ的偏导数，表示磁强计误差模型中g(δσ)对状态误差δσ的偏导数，

所述磁强计误差模型中g(δσ)对状态误差δσ的偏导数的具体公式如下：

所述步骤5)中利用扩展kalman滤波方法得到修正rodrigues参数的最优估计的具体过程为：利用得到的非线性状态误差方程和观测方程，结合线性化状态模型与观测模型，利用扩展kalman滤波方法，递推得到修正rodrigues参数的最优估计，作为姿态结果供制导控制使用，实现任意姿态的mems和磁强计组合定姿。

本发明与现有技术相比的优点是：本项目提出的mems/磁强计组合定姿方法，基于修正的rodrigues参数建立状态方程和观测方程，利用扩展kalman滤波方法得到误差的最优估计，实现了任意姿态下无初始信息的组合定姿，有效地解决了无依托发射中惯组初始对准问题，无需配备地面瞄准设施，同时大大缩短发射准备时间。

附图说明

图1为本发明方法流程图；

图2为mems陀螺磁强计组合定姿误差示意图。

具体实施方式

本项目提出了一种在无任何初始姿态信息情况下，仅利用惯组与三轴磁强计获得导弹武器系统的三轴姿态信息的方法。在无任何先验姿态信息情况下，姿态误差不再是小偏差，不能用传统的小偏差线性方程来建立姿态误差模型。描述非线性姿态误差通常采用四元数模型，四元数有四个变量，而仅有三个是独立的，因而存在冗余问题，会使状态误差协方差阵产生奇异，同时采用四元数解算还存在规范化的问题。传统的处理方法为采用四元数的矢量部分进行建模，忽略四元数中的标量部分，即旋转角大小，即假定四元数误差为小角度。但实际中惯组初始姿态偏差较大，上述模型不再适应，需要建立任意初始大姿态角偏差均能适应的姿态误差模型。

本项目将修正rodrigues参数引入到姿态确定中，将四元数的四个参量转化为三个参量来描述姿态角误差，保留四元数的矢量形式和幅值，同时避免采用姿态角描述存在的奇异性问题，在无任何初始姿态信息的情况下，即可完成三轴姿态确定。

1、定义修正的rodrigues参数，将四元数四个量转化为三个量来描述姿态，保证三个参数之间的独立性，同时避免传统四元数存在的误差协方差阵奇异问题和规范化问题。

修正的rodrigues参数：

式中，为四元数；为旋转轴单位向量，θ为转过的角度。通过tan(θ4)来描述旋转角度，该修正rodrigues参数能够适应-π～π的范围，即可适应任意旋转角度的四元数。

四元数偏差其中qc为四元数的观测值，与之对应的修正rodrigues误差可以表示为：

式中，δqv表示四元数误差矢量部分；同样地，由式(2)可得到四元数误差如下：

2、建立基于修正rodrigues参数的姿态误差状态方程：

对式(2)求微分方程得到姿态误差方程如下：

四元数的误差方程如下：

其中陀螺测量误差为惯组本体坐标系相对于参考坐标系的旋转角速度在惯组本体系下的投影，为惯组输出的测量值。

将式(5)、(3)代入式(4)：

3、建立磁强计的观测模型：

以磁场强度矢量相减建立磁强计的观测模型：

式中，为通过地磁场模型、姿态四元数计算得到的本体四元数，为磁强计测量值；

将式(3)代入式(7)可得：

4、对非线性姿态误差状态模型和磁强计的观测模型进行线化处理：

对式(6)进行线性化，得到状态误差的线性化模型：

其中：

对式(8)进行线性化，得到磁强计线性化观测模型：

其中

其中，其计算公式如下：

5、对误差模型进行kalman滤波估计

根据上述建立的状态方程与观测模型，采用非线性kalman滤波方法，对任意初始姿态下mems惯组+磁强计组合系统进行估计，实现组合定姿。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

根据上述建立的状态模型与观测模型，采用非线性kalman滤波方式，对任意初始姿态偏差下mems惯组/磁强计组合定姿技术进行验证，仿真结果如图2所示，结果表明组合定姿模型正确，方案可行。