[发明专利]一种基于克雷洛夫角奇异条件下的姿态角解算方法

权利要求书

1.一种基于克雷洛夫角奇异条件下的姿态角解算方法，其特征在于：包括如下步骤：

步骤一、在飞行器上固定安装陀螺仪组，陀螺仪组形成捷联式惯性系统；建立捷联式惯性系统对应的本体坐标系O′X′Y′Z′；建立飞行器对应的导航坐标系OXYZ；两个坐标系的原点重合；

步骤二、采用克雷洛夫角旋转法，将导航坐标系OXYZ依次转动角、ψ角和γ角后与本体坐标系O′X′Y′Z′重合；ψ和γ即为基于克雷洛夫角奇异条件下的3个姿态角；通过捷联式惯性系统测量得到本体坐标系O′X′Y′Z′相对于导航坐标系OXYZ的旋转角速度其中，为克雷洛夫角对应X轴旋转角速度；为克雷洛夫角对应Y轴旋转角速度；为克雷洛夫角对应Z轴旋转角速度；

步骤三、计算3个克雷洛夫角对应的角速度角速度的计算方法为：

步骤四、计算从导航坐标系OXYZ至本体坐标系O′X′Y′Z′的变换矩阵变换矩阵的计算方法为：

步骤五、测量步骤二中导航坐标系OXYZ旋转前的初始角度ψ₀和γ₀；对和的取值进行判断，根据判断结果计算基于克雷洛夫角奇异条件下的3个姿态角ψ、γ和变换矩阵

根据和的取值，计算3个姿态角ψ、γ和变换矩阵的方法为：

S1、当和都为零时，则姿态角为姿态角ψ为ψ₀；根据公式(1)得到角速度无奇点，对角速度方程求积分，即可得到姿态角γ；

计算变换矩阵将姿态角ψ₀和γ代入公式(2)，即可求得变换矩阵

S2、当和中至少有1个为非零时，3个克雷洛夫运动学方程为：

式中，

对3个克雷洛夫运动学方程求积分，即可得到姿态角ψ和γ；对姿态角ψ进行判断，并根据判断结果计算变换矩阵

2.根据权利要求1所述的一种基于克雷洛夫角奇异条件下的姿态角解算方法，其特征在于：所述步骤一中，

本体坐标系O′X′Y′Z′的建立方法为：原点O为飞行器的中心，X′方向指向飞行器的纵轴，Z′方向指向飞行器的横轴，Y′方向由右手定则确定；

导航坐标系OXYZ的建立方法为：原点为飞行器的中心，方向指向东，方向指向北，方向由右手定则确定。

3.根据权利要求2所述的一种基于克雷洛夫角奇异条件下的姿态角解算方法，其特征在于：所述步骤二中，导航坐标系OXYZ转动的具体过程为：

S1、将导航坐标系OXYZ绕OZ轴旋转角，到达OLNZ坐标系；

S2、将OLNZ坐标系绕ON轴旋转ψ角；到达OX′NM坐标系；

S3、将OX′NM坐标系绕OX′轴旋转γ角；到达本体坐标系O′X′Y′Z′。

4.根据权利要求3所述的一种基于克雷洛夫角奇异条件下的姿态角解算方法，其特征在于：所述步骤五的S2中，根据姿态角ψ的判断结果计算变换矩阵的具体方法为：

当89°＜ψ＜90°时，sinψ＝1，tanψ0；γ的初始值为γ₀，为保证系统稳定，则cos(γ+α)＝-1，即γ＝180°-α；其中，γ的变化量Δγ为Δγ＝180°-α-γ₀；的稳态值为代入公式(2)，得到：

当90°＜ψ＜91°时，sinψ＝1，tanψ0；γ的初始值为γ₀，为保证系统稳定，则cos(γ+α)＝1，即γ＝-α；其中，γ的变化量Δγ为Δγ＝-α-γ₀；的稳态值为代入公式(2)，得到：

当-91°＜ψ＜90°时，sinψ＝-1，tanψ＞0；γ的初始值为γ₀，为保证系统稳定，则有cos(γ+α)＝-1，即γ＝180°-α；其中，γ的变化量Δγ为Δγ＝180°-α-γ₀；

的稳态值为代入公式(2)，得到：

当-90°＜ψ＜-89°时，sinψ＝-1，tanψ＜0；γ的初始值为γ₀，为保证系统稳定，则有cos(γ+α)＝1，即γ＝-α；其中，γ的变化量Δγ为Δγ＝-α-γ₀；的稳态值为代入公式(2)，得到：