[发明专利]一种摆动喷管姿态角解算方法

权利要求书

1.一种摆动喷管姿态角解算方法，其特征在于，所述方法的具体步骤为：

步骤1：根据MEMS陀螺仪在摆动喷管上的初始安装角度，确定初始姿态角，从而确定初始四元数；

步骤2：将MEMS陀螺仪固联在摆动喷管上，通过初始的姿态角确定摆动喷管坐标系到MEMS陀螺仪测量坐标系的转换矩阵；

步骤3：在摆动喷管测试过程中，采集MEMS陀螺仪测量的数据；

步骤4：对步骤3采集的数据，通过转换矩阵转换到摆动喷管坐标系；

步骤5：根据步骤1的初始四元数以及步骤4得到的转换到摆动喷管坐标系的数据，进行四元数更新计算；

步骤6：根据四元数与姿态角的对应关系，进行姿态角解算；

步骤1具体为：

步骤2.1：摆动喷管坐标系定义

摆动喷管水平放置，其中ObXb指向喷管尾部；ObYb与ObXb垂直，指向正上方；ObZb与ObXb、ObYb构成右手坐标系；

步骤2.2：MEMS陀螺仪测量坐标系定义

Oc1Z c1与ObZb轴重合；Oc1Xc1、Oc1Yc1构成的平面Oc1Xc1Yc1，与ObXb、ObYb构成的平面ObXbYb重合，Oc1Xc1与ObXb的夹角为Oc1Yc1与ObYb的夹角为

步骤2.3：摆动喷管坐标系与MEMS陀螺仪测量坐标系之间有一个初始角度根据坐标的转换关系，可以得到摆动喷管坐标系到MEMS陀螺仪测量坐标系的转换关系，并且用四元数进行标识：

其中：d₁为四元数自转轴X向分量；d₂为四元数自转轴Y向分量；d₃为四元数自转轴Z向分量；q₀、q₁、q₂、q₃为初始四元数；

步骤5中四元数更新计算方法为：

其中：θ_xb为X方向摆动喷管坐标系转换值；θ_yb为Y方向摆动喷管坐标系转换值；θ_zb为Z方向摆动喷管坐标系转换值；θ为测量值合成结果；q₀₁、q₁₁、q₂₁、q₃₁为更新后的四元数。

2.根据权利要求1所述的一种摆动喷管姿态角解算方法，其特征在于，步骤2中所述摆动喷管坐标系到MEMS陀螺仪测量坐标系的转换矩阵为：

3.根据权利要求1所述的一种摆动喷管姿态角解算方法，其特征在于，步骤3中所述MEMS陀螺仪测量的数据包括θ_xc1、θ_yc1、θ_zc1，其中θ_xc1为X方向MEMS陀螺仪测量值；θ_yc1为Y方向MEMS陀螺仪测量值；θ_zc1为Z方向MEMS陀螺仪测量值。

4.根据权利要求2所述的一种摆动喷管姿态角解算方法，其特征在于，步骤4中所述数据通过转换矩阵转换到摆动喷管坐标系的具体计算方法为：

其中，θ_xc1为X方向MEMS陀螺仪测量值；θ_yc1为Y方向MEMS陀螺仪测量值；θ_zc1为Z方向MEMS陀螺仪测量值；θ_xb为X方向摆动喷管坐标系转换值；θ_yb为Y方向摆动喷管坐标系转换值；θ_zb为Z方向摆动喷管坐标系转换值。

5.根据权利要求1所述的一种摆动喷管姿态角解算方法，其特征在于，步骤6中姿态角解算方法具体为：

其中：为俯仰角；ψ为偏航角；γ为滚转角。