一种变轨机动飞机全姿态惯性导航的姿态控制方法与流程

本发明涉及一种变轨机动飞机全姿态惯性导航的姿态控制方法，尤其涉及基于四元数解算的捷联式惯性系统全姿态解算方法，主要用于实现全姿态高精度导航的航空、航天领域。

背景技术：

惯性导航广泛应用于导弹、飞机、舰船和兵器等领域，主要作用是实时确定载体相对导航系的位置、速度和姿态信息。捷联式惯性系统与载体直接固连，通过陀螺仪测量角速度并经数学解算后给出三个姿态角的值。但对于飞机在空间做全姿态飞行时，特别是当俯仰角跨越±90°时，姿态角的计算方法尚未见到。

确定姿态信息的方法有方向余弦运动学方程、欧拉-克雷洛夫角运动学方程以及四元数运动学方程。方向余弦运动学方程的优点是直接可求得动系和定系之间的坐标变换矩阵，缺点是变换矩阵的参数为9个、联系式为6个，计算量较大，在工程中应用较少。

目前，在工程中应用最多的是四元数运动学方程，其优点是参数数目只有4个，其缺点是4个参数都只是中间变量，需要通过解算给出坐标变换矩阵以及姿态角。

当载体运动机动范围有限时，比如，飞机俯仰角为-90°至+90°的范围内飞行，可采用四元数法进行姿态角解算。随着载体长航时、变轨等机动能力的提高，要求三个姿态角的运动范围由±90°扩展至±180°，即全姿态。此时，采用四元数求解将存在以下两个问题：(1)多值问题，即由坐标变换矩阵求解姿态角时不存在唯一解；(2)奇异值问题。

技术实现要素：

本发明的技术解决问题：克服现有技术的不足，提供一种变轨机动飞机全姿态惯性导航的姿态控制方法，该方法实现了捷联式惯性系统在奇异条件下的姿态角解算，确保了姿态角与载体运动状态的一致性，提高了捷联式惯性系统的全姿态适应能力。

本发明的上述目的通过以下技术方案实现：一种变轨机动飞机全姿态惯性导航的姿态控制方法，将捷联式惯性系统安装在变轨机动飞机上，所述捷联式惯性系统在所述捷联式惯性系统的本体上安装三个正交的陀螺仪和三个正交的加速度计，对应的本体坐标系为ox′y′z′，其中，ox′轴表示载体的纵轴，oz′轴表示载体的横轴，oy′轴与其它两轴组成右手坐标系；建立描述载体运动姿态角对应的导航坐标系为oxyz，导航坐标系取东北天地理坐标系，其中，ox指东，oy轴指北，oz轴指天；两个坐标系的原点重合；

所述变轨机动飞机全姿态惯性导航的姿态控制方法包括如下步骤：

(1)根据本体坐标系相对导航坐标系的初始偏航角初始俯仰角ψ和初始滚转角γ得到坐标变换矩阵及四元数初值q0；其中，的表达式为：

其中，为本体坐标系oz′轴的单位矢量，为本体坐标系oy′轴的单位矢量，为本体坐标系ox′轴的单位矢量，n为导航坐标系，b为本体坐标系；

(2)预设采样时间为δt，实时获取捷联式惯性系统测量的本体相对导航坐标系的角速度其中，ωx′、ωy′、ωz′为沿本体坐标系ox′y′z′三个轴ox′、oy′、oz′的角速度分量；

(3)当ψ角在[-180°，-90°-ε]、[-90°+ε，90°-ε]或[90°+ε，180°]区间时，根据四元数法得到第一坐标变换矩阵以及第一偏航角第一俯仰角ψ'和第一滚转角γ'；其中，ε为临近±90°的域值；

(4)当ψ角在(-90°-ε，-90°+ε)或(90°-ε，90°+ε)区间时，根据克雷洛夫角微分方程得到第二坐标变换矩阵以及第二偏航角第二俯仰角ψ”和第二滚转角γ”；

(5)将步骤(3)得到的第一偏航角第一俯仰角ψ'和第一滚转角γ'或步骤(4)得到的第二偏航角第二俯仰角ψ”和第二滚转角γ”用于变轨机动飞机的姿态控制，将步骤(3)得到的第一坐标变换矩阵或步骤(4)得到的第二坐标变换矩阵用于惯性导航的速度和位置解算。

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(3)中，当ψ角在[-180°，-90°-ε]、[-90°+ε，90°-ε]或[90°+ε，180°]区间时，根据四元数法得到第一坐标变换矩阵以及第一偏航角第一俯仰角ψ'和第一滚转角γ'包括如下步骤：

(3.1)依据步骤(2)的角速度ωx′、ωy′、ωz′计算的瞬时转角为：

(3.2)依据步骤(1)的四元数初值q0与步骤(3.1)的δθ，得到第一四元数q：

式中，第一四元数可写为向量表达式

其中，δθ为采样时间δt内本体坐标系相对导航坐标系的转角，λ为四元数的标量，ρ1为四元数沿本体坐标系ox′轴的分量，ρ2为四元数沿本体坐标系oy′轴的分量，ρ3为四元数沿本体坐标系oz′轴的分量。

(3.3)根据步骤(3.2)的第一四元数的各元素λ、ρ1、ρ2、ρ3，得到从导航坐标系至本体坐标系的第一坐标变换矩阵为

式中，b为本体坐标系，n为导航坐标系；

(3.4)根据步骤(3.3)的第一坐标变换矩阵得到第一偏航角第一俯仰角ψ'和第一滚转角γ'。

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(3.4)中，根据步骤(3.3)的第一坐标变换矩阵得到第一偏航角第一俯仰角ψ'和第一滚转角γ'包括如下步骤：

(3.4.1)当ψ∈[-180°，-90°-ε]或ψ∈[90°+ε，180°]时，有

(3.4.2)当ψ∈[-90°+ε，90°-ε]时，有

ψ'＝-arcsin(2(ρ1ρ3-λρ2))

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(4)中，当ψ角在(-90°-ε，-90°+ε)或(90°-ε，90°+ε)区间时，根据克雷洛夫角微分方程得到第二坐标变换矩阵以及第二偏航角第二俯仰角ψ”和第二滚转角γ”包括如下步骤：

(4.1)根据步骤(1)的初始偏航角初始俯仰角ψ和初始滚转角γ和步骤(2)的角速度，获得第二偏航角第二俯仰角ψ”和第二滚转角γ”；

(4.2)根据步骤(4.1)的第二偏航角第二俯仰角ψ”和第二滚转角γ”得到第二坐标变换矩阵

(4.3)根据步骤(4.1)的第二偏航角第二俯仰角ψ”和第二滚转角γ”得到第二四元数q”：

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(4.1)中，第二偏航角第二俯仰角ψ”和第二滚转角γ”的具体计算方法如下：

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(4.2)中，第二坐标变换矩阵为：

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(4.3)中，第二四元数q”为：

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(3)中，ε确定原则为：ε＝2×max(ωy′,ωz′)×δt。

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(2)中，ωx′、ωy′、ωz′由速率陀螺仪测量得到。

上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(1)中，ψ、γ的取值范围均为-180°～+180°。

本发明与现有技术相比的优点如下：

(1)、本发明给出的一种变轨机动飞机全姿态惯性导航的姿态控制方法，完全覆盖了3个姿态角在任意象限的情况，克服了原有技术在ψ角为±90°时的所谓“退化”而不能实现姿态角解算问题；

(2)、本发明给出了一种变轨机动飞机全姿态惯性导航的姿态控制方法，把ψ角应用范围扩展到了(-180°，+180°)，克服了原有技术对ψ角限制为(-90°，+90°)不能实现全姿态解算问题；

(3)本发明给出了一种变轨机动飞机全姿态惯性导航的姿态控制方法，计算的三个姿态角在(-180°，+180°)的值唯一，不存在多解的问题，克服了原技术存在多解的问题，；

(4)本发明给出了一种变轨机动飞机全姿态惯性导航的姿态控制方法，可直接精确给出本体相对导航坐标系的姿态角，计算过程简化，可实现快速解算。

附图说明

图1为捷联式惯性系统中以克雷洛夫角描述的本体相对导航坐标系的关系示意图；

图2为本发明示例中本体相对导航坐标系的关系示意图；

图3为本发明示例中三个克雷洛夫角随时间的理想变化过程；

图4为本发明示例中采用四元数坐标变换矩阵解算的三个克雷洛夫角随时间的变化过程；

图5为本发明示例中采用本发明技术解算的三个克雷洛夫角随时间的变化过程；

图6为本发明示例中采用本发明技术解算的三个克雷洛夫角随时间的变化过程；

图7为本发明的变轨机动飞机全姿态惯性导航的姿态控制方法的流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的描述：

捷联式惯性系统在所述捷联式惯性系统的本体上安装三个正交的陀螺仪和三个正交的加速度计，对应的本体坐标系为ox′y′z′，其中，ox′轴表示载体的纵轴，oz′轴表示载体的横轴，oy′轴与其它两轴组成右手坐标系；建立描述载体运动姿态角对应的导航坐标系为oxyz，导航坐标系取东北天地理坐标系，其中，ox指东，oy轴指北，oz轴指天；两个坐标系的原点重合，关系如图1所示。

本实施例为了解决以下两个问题：

(1)奇异值问题

另外，当ψ＝90°时，如图2所示，坐标变换矩阵为

从上式可以看出，不能求出姿态角γ独立的值，而只能求出二者之差

满足该差值的和γ有无穷多个，因此，提出可把其中一个姿态角设置为零，比如而另外一个姿态角取为但这种方法的缺点是的真值如果不为零时，则两个角度的值都与真值不相符。

(2)多值问题，即由坐标变换矩阵求解姿态角时不存在唯一解。

采用四元数法时不能直接求得姿态角，而是通过坐标变换矩阵的各元素计算出姿态角。由四元数求出的坐标变换矩阵为

式中，b为动系，n为定系；λ、ρ1、ρ2、ρ3分别为四元数的4个元素。

在求三个姿态角ψ、γ时，列写出用姿态角描述的坐标变换矩阵为

根据现有公开的方法，主要是比较式(1)和式(2)来求解姿态角。但是，存在的一个主要问题是，一个坐标变换矩阵可以用两组不同的姿态角表述，比如，就可用另一组姿态角π-ψ、γ-π表述为

因此，根据求解的姿态角ψ、γ不唯一。为此，都把ψ限定在(-90°，+90°)，根据式(2)求解的3个姿态角的主值为

由于和γ的取值范围定义在[-180°，+180°],根据式(2)对公式(4)进行分段处理可得到精确值。从以上分析可以看出，正因为把ψ限定在(-90°，+90°)范围内，从而使载体(飞机、导弹等)的运动不能实现全姿态的功能。

比如，飞机在做俯仰角(-180°，+180°)翻滚动作时，设姿态角ψ、γ的初值为0°，飞机的角速度时，三个姿态角在翻滚一圈的变换过程如图3所示。但是，采用四元数解算的三个姿态角如图4所示，可以看出，求解的姿态角ψ在变化过程中不能跨过+90°和-90°，和γ会分别变化180°，与飞行轨迹的理论值之间不吻合。这说明，采用原有的四元数解算姿态角的方法存在缺陷和不足。

本实施例提供一种变轨机动飞机全姿态惯性导航的姿态控制方法，基于与载体固连的捷联式惯性系统实现，所述捷联式惯性系统对应的本体坐标系(b系，为动系)为ox′y′z′；描述载体运动姿态角对应的导航坐标系(n系，为定系)为oxyz；两个坐标系的原点重合。

在解算姿态角时，采用克雷洛夫角描述两个坐标系之间的关系，导航坐标系经过三次转动后到达本体坐标系，具体转动过程为：导航坐标系oxyz绕oz轴转动角，到达olnz；olnz再绕on轴转动ψ角，到达ox′nm；最后，ox′nm绕ox′轴转动γ角，到达ox′y′z′。

所述全姿态惯性导航的姿态角解算方法实现步骤如下：

(1)根据本体坐标系相对导航坐标系的初始偏航角初始俯仰角ψ和初始滚转角γ得到坐标变换矩阵及四元数初值q0；其中，的表达式为

其中，为本体坐标系oz′轴的单位矢量，为本体坐标系oy′轴的单位矢量，为本体坐标系ox′轴的单位矢量，n为导航坐标系，b为本体坐标系。

(2)预设采样时间为δt，实时获取捷联式惯性系统测量的本体相对导航坐标系的角速度其中，ωx′、ωy′、ωz′为沿本体坐标系ox′y′z′三个轴ox′、oy′、oz′的角速度分量；

(3)当ψ角在[-180°，-90°-ε]、[-90°+ε，90°-ε]或[90°+ε，180°]区间时，根据四元数法得到第一坐标变换矩阵以及第一偏航角第一俯仰角ψ'和第一滚转角γ'；其中，ε为临近±90°的域值。

(3.1)依据步骤(2)的角速度ωx′、ωy′、ωz′计算的瞬时转角为：

(3.2)依据步骤(1)的四元数初值q0与步骤(3.1)的δθ，得到第一四元数q：

式中，第一四元数可写为向量表达式

其中，δθ为采样时间δt内本体坐标系相对导航坐标系的转角，λ为四元数的标量，ρ1为四元数沿本体坐标系ox′轴的分量，ρ2为四元数沿本体坐标系oy′轴的分量，ρ3为四元数沿本体坐标系oz′轴的分量。

(3.3)根据步骤(3.2)的第一四元数的各元素λ、ρ1、ρ2、ρ3，得到从导航坐标系至本体坐标系的第一坐标变换矩阵为

式中，b为本体坐标系，n为导航坐标系；

(3.4)根据步骤(3.3)的第一坐标变换矩阵得到第一偏航角第一俯仰角ψ'和第一滚转角γ'，解算方法为

(3.4.1)当ψ∈[-180°，-90°-ε]或ψ∈[90°+ε，180°]时，有

(3.4.2)当ψ∈[-90°+ε，90°-ε]时，有

ψ'＝-arcsin(2(ρ1ρ3-λρ2))

(4)当ψ角在(-90°-ε，-90°+ε)或(90°-ε，90°+ε)区间时，根据克雷洛夫角微分方程得到第二坐标变换矩阵以及第二偏航角第二俯仰角ψ”和第二滚转角γ”；

(4.1)根据步骤(1)的初始偏航角初始俯仰角ψ和初始滚转角γ和步骤(2)的角速度，获得第二偏航角第二俯仰角ψ”和第二滚转角γ”；具体计算方法如下

(4.2)根据步骤(4.1)的第二偏航角第二俯仰角ψ”和第二滚转角γ”得到第二坐标变换矩阵

(4.3)根据步骤(4.1)的第二偏航角第二俯仰角ψ”和第二滚转角γ”得到第二四元数q”

(5)将步骤(3)得到的第一偏航角第一俯仰角ψ'和第一滚转角γ'或步骤(4)得到的第二偏航角第二俯仰角ψ”和第二滚转角γ”用于变轨机动飞机的姿态控制，将步骤(3)得到的第一坐标变换矩阵或步骤(4)得到的第二坐标变换矩阵用于惯性导航的速度和位置解算。

上述实施例中，在步骤(1)中初始姿态角ψ、γ由外部主惯导或光学瞄准传递对准给出。

上述实施例中，在步骤(3)中的ε确定原则为

ε＝2×max(ωy′,ωz′)×δt

上述实施例中，在步骤(3)中计算姿态角时所用的角速度ωx′、ωy′、ωz′由速率陀螺仪测量得到。

上述实施例中，在步骤(1)中，转动角度ψ、γ的取值范围为-180°～+180°。

为形象说明本发明提供的基于克雷洛夫角奇异条件下的姿态角解算方法，优选的实施例为：

例1：飞机在做俯仰角(-180°，+180°)翻滚动作时，设姿态角ψ、γ的初值为0°，飞机的角速度时，三个姿态角在翻滚一圈的变换过程如图3所示。但是，采用四元数解算的三个姿态角如图4所示，可以看出，求解的姿态角ψ在变化过程中不能跨过+90°和-90°，和γ会分别变化180°，与飞行轨迹的理论值之间不吻合。这说明，采用原有的四元数解算姿态角的方法存在缺陷和不足。而采用本发明给出的方法解算的三个姿态角如图5所示，与图3比较可以看出，二者一致，说明了本发明技术的有效。

例2：飞机在做俯仰角(-180°，+180°)翻滚动作时，设姿态角和γ的初值为0°，ψ的初值为135°，飞机的角速度时，三个姿态角在翻滚一圈的变换过程如图6所示。可以看出，本发明给出的方法可有效覆盖全姿态。

以上所述，仅为本发明一个具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

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