CN111780749A - 一种变轨机动飞机全姿态惯性导航的姿态控制方法 - Google Patents

一种变轨机动飞机全姿态惯性导航的姿态控制方法 Download PDF

Info

Publication number
CN111780749A
CN111780749A CN202010457154.9A CN202010457154A CN111780749A CN 111780749 A CN111780749 A CN 111780749A CN 202010457154 A CN202010457154 A CN 202010457154A CN 111780749 A CN111780749 A CN 111780749A
Authority
CN
China
Prior art keywords
angle
attitude
coordinate system
full
quaternion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010457154.9A
Other languages
English (en)
Other versions
CN111780749B (zh
Inventor
魏宗康
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Aerospace Control Instrument Institute
Original Assignee
Beijing Aerospace Control Instrument Institute
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Aerospace Control Instrument Institute filed Critical Beijing Aerospace Control Instrument Institute
Priority to CN202010457154.9A priority Critical patent/CN111780749B/zh
Publication of CN111780749A publication Critical patent/CN111780749A/zh
Application granted granted Critical
Publication of CN111780749B publication Critical patent/CN111780749B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种变轨机动飞机全姿态惯性导航的姿态控制方法，针对飞机做全姿态角运动时不同角度区间分别采用坐标变换矩阵解算姿态角、克雷洛夫角速率积分求解姿态角等，求解出描述机体相对导航系的3个姿态角。本发明给出的3个姿态角不受象限限制，可实现全姿态描述，首次实现了捷联式惯性系统全姿态的真实描述，且算法简单、工程实现方便。

Description

一种变轨机动飞机全姿态惯性导航的姿态控制方法
技术领域
本发明涉及一种变轨机动飞机全姿态惯性导航的姿态控制方法，尤其涉及基于四元数解算的捷联式惯性系统全姿态解算方法，主要用于实现全姿态高精度导航的航空、航天领域。
背景技术
惯性导航广泛应用于导弹、飞机、舰船和兵器等领域，主要作用是实时确定载体相对导航系的位置、速度和姿态信息。捷联式惯性系统与载体直接固连，通过陀螺仪测量角速度并经数学解算后给出三个姿态角的值。但对于飞机在空间做全姿态飞行时，特别是当俯仰角跨越±90°时，姿态角的计算方法尚未见到。
确定姿态信息的方法有方向余弦运动学方程、欧拉-克雷洛夫角运动学方程以及四元数运动学方程。方向余弦运动学方程的优点是直接可求得动系和定系之间的坐标变换矩阵，缺点是变换矩阵的参数为9个、联系式为6个，计算量较大，在工程中应用较少。
目前，在工程中应用最多的是四元数运动学方程，其优点是参数数目只有4个，其缺点是4个参数都只是中间变量，需要通过解算给出坐标变换矩阵以及姿态角。
当载体运动机动范围有限时，比如，飞机俯仰角为-90°至+90°的范围内飞行，可采用四元数法进行姿态角解算。随着载体长航时、变轨等机动能力的提高，要求三个姿态角的运动范围由±90°扩展至±180°，即全姿态。此时，采用四元数求解将存在以下两个问题：(1)多值问题，即由坐标变换矩阵求解姿态角时不存在唯一解；(2)奇异值问题。
发明内容
本发明的技术解决问题：克服现有技术的不足，提供一种变轨机动飞机全姿态惯性导航的姿态控制方法，该方法实现了捷联式惯性系统在奇异条件下的姿态角解算，确保了姿态角与载体运动状态的一致性，提高了捷联式惯性系统的全姿态适应能力。
本发明的上述目的通过以下技术方案实现：一种变轨机动飞机全姿态惯性导航的姿态控制方法，将捷联式惯性系统安装在变轨机动飞机上，所述捷联式惯性系统在所述捷联式惯性系统的本体上安装三个正交的陀螺仪和三个正交的加速度计，对应的本体坐标系为OX′Y′Z′，其中，OX′轴表示载体的纵轴，OZ′轴表示载体的横轴，OY′轴与其它两轴组成右手坐标系；建立描述载体运动姿态角对应的导航坐标系为OXYZ，导航坐标系取东北天地理坐标系，其中，OX指东，OY轴指北，OZ轴指天；两个坐标系的原点重合；
所述变轨机动飞机全姿态惯性导航的姿态控制方法包括如下步骤：
(1)根据本体坐标系相对导航坐标系的初始偏航角
Figure BDA0002509773460000021
初始俯仰角ψ和初始滚转角γ得到坐标变换矩阵
Figure BDA0002509773460000022
及四元数初值q0；其中，
Figure BDA0002509773460000023
的表达式为：
Figure BDA0002509773460000024
Figure BDA0002509773460000025
其中，
Figure BDA0002509773460000026
为本体坐标系OZ′轴的单位矢量，
Figure BDA0002509773460000027
为本体坐标系OY′轴的单位矢量，
Figure BDA0002509773460000028
为本体坐标系OX′轴的单位矢量，n为导航坐标系，b为本体坐标系；
(2)预设采样时间为ΔT，实时获取捷联式惯性系统测量的本体相对导航坐标系的角速度
Figure BDA0002509773460000029
其中，ωx′、ωy′、ωz′为沿本体坐标系OX′Y′Z′三个轴OX′、OY′、OZ′的角速度分量；
(3)当ψ角在[-180°，-90°-ε]、[-90°+ε，90°-ε]或[90°+ε，180°]区间时，根据四元数法得到第一坐标变换矩阵
Figure BDA0002509773460000031
以及第一偏航角
Figure BDA0002509773460000032
第一俯仰角ψ'和第一滚转角γ'；其中，ε为临近±90°的域值；
(4)当ψ角在(-90°-ε，-90°+ε)或(90°-ε，90°+ε)区间时，根据克雷洛夫角微分方程得到第二坐标变换矩阵
Figure BDA0002509773460000033
以及第二偏航角
Figure BDA0002509773460000034
第二俯仰角ψ”和第二滚转角γ”；
(5)将步骤(3)得到的第一偏航角
Figure BDA0002509773460000035
第一俯仰角ψ'和第一滚转角γ'或步骤(4)得到的第二偏航角
Figure BDA0002509773460000036
第二俯仰角ψ”和第二滚转角γ”用于变轨机动飞机的姿态控制，将步骤(3)得到的第一坐标变换矩阵
Figure BDA0002509773460000037
或步骤(4)得到的第二坐标变换矩阵
Figure BDA0002509773460000038
用于惯性导航的速度和位置解算。
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(3)中，当ψ角在[-180°，-90°-ε]、[-90°+ε，90°-ε]或[90°+ε，180°]区间时，根据四元数法得到第一坐标变换矩阵
Figure BDA0002509773460000039
以及第一偏航角
Figure BDA00025097734600000310
第一俯仰角ψ'和第一滚转角γ'包括如下步骤：
(3.1)依据步骤(2)的角速度ωx′、ωy′、ωz′计算的瞬时转角为：
Figure BDA00025097734600000311
(3.2)依据步骤(1)的四元数初值q0与步骤(3.1)的Δθ，得到第一四元数q：
Figure BDA00025097734600000312
式中，第一四元数
Figure BDA00025097734600000313
可写为向量表达式
Figure BDA00025097734600000314
其中，Δθ为采样时间ΔT内本体坐标系相对导航坐标系的转角，λ为四元数的标量，ρ1为四元数沿本体坐标系OX′轴的分量，ρ2为四元数沿本体坐标系OY′轴的分量，ρ3为四元数沿本体坐标系OZ′轴的分量。
(3.3)根据步骤(3.2)的第一四元数的各元素λ、ρ1、ρ2、ρ3，得到从导航坐标系至本体坐标系的第一坐标变换矩阵
Figure BDA0002509773460000041
为
Figure BDA0002509773460000042
式中，b为本体坐标系，n为导航坐标系；
(3.4)根据步骤(3.3)的第一坐标变换矩阵
Figure BDA0002509773460000043
得到第一偏航角
Figure BDA0002509773460000044
第一俯仰角ψ'和第一滚转角γ'。
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(3.4)中，根据步骤(3.3)的第一坐标变换矩阵
Figure BDA0002509773460000045
得到第一偏航角
Figure BDA0002509773460000046
第一俯仰角ψ'和第一滚转角γ'包括如下步骤：
(3.4.1)当ψ∈[-180°，-90°-ε]或ψ∈[90°+ε，180°]时，有
Figure BDA0002509773460000047
Figure BDA0002509773460000048
Figure BDA0002509773460000051
(3.4.2)当ψ∈[-90°+ε，90°-ε]时，有
ψ'＝-arcsin(2(ρ1ρ3-λρ2))
Figure BDA0002509773460000052
Figure BDA0002509773460000053
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(4)中，当ψ角在(-90°-ε，-90°+ε)或(90°-ε，90°+ε)区间时，根据克雷洛夫角微分方程得到第二坐标变换矩阵
Figure BDA0002509773460000061
以及第二偏航角
Figure BDA0002509773460000062
第二俯仰角ψ”和第二滚转角γ”包括如下步骤：
(4.1)根据步骤(1)的初始偏航角
Figure BDA0002509773460000063
初始俯仰角ψ和初始滚转角γ和步骤(2)的角速度，获得第二偏航角
Figure BDA0002509773460000064
第二俯仰角ψ”和第二滚转角γ”；
(4.2)根据步骤(4.1)的第二偏航角
Figure BDA0002509773460000065
第二俯仰角ψ”和第二滚转角γ”得到第二坐标变换矩阵
Figure BDA0002509773460000066
(4.3)根据步骤(4.1)的第二偏航角
Figure BDA0002509773460000067
第二俯仰角ψ”和第二滚转角γ”得到第二四元数q”：
Figure BDA0002509773460000068
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(4.1)中，第二偏航角
Figure BDA0002509773460000069
第二俯仰角ψ”和第二滚转角γ”的具体计算方法如下：
Figure BDA00025097734600000610
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(4.2)中，第二坐标变换矩阵
Figure BDA00025097734600000611
为：
Figure BDA00025097734600000612
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(4.3)中，第二四元数q”为：
Figure BDA00025097734600000613
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(3)中，ε确定原则为：ε＝2×max(ωy′,ωz′)×ΔT。
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(2)中，ωx′、ωy′、ωz′由速率陀螺仪测量得到。
上述变轨机动飞机全姿态惯性导航的姿态控制方法中，在步骤(1)中，
Figure BDA0002509773460000071
ψ、γ的取值范围均为-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所示，坐标变换矩阵为
Figure BDA0002509773460000081
从上式可以看出，不能求出姿态角
Figure BDA0002509773460000082
γ独立的值，而只能求出二者之差
Figure BDA0002509773460000083
满足该差值的
Figure BDA0002509773460000084
和γ有无穷多个，因此，提出可把其中一个姿态角设置为零，比如
Figure BDA0002509773460000085
而另外一个姿态角取为
Figure BDA0002509773460000086
但这种方法的缺点是
Figure BDA0002509773460000087
的真值如果不为零时，则两个角度的值都与真值不相符。
(2)多值问题，即由坐标变换矩阵求解姿态角时不存在唯一解。
采用四元数法时不能直接求得姿态角，而是通过坐标变换矩阵的各元素计算出姿态角。由四元数求出的坐标变换矩阵为
Figure BDA0002509773460000088
式中，b为动系，n为定系；λ、ρ1、ρ2、ρ3分别为四元数的4个元素。
在求三个姿态角
Figure BDA0002509773460000089
ψ、γ时，列写出用姿态角描述的坐标变换矩阵为
Figure BDA0002509773460000091
根据现有公开的方法，主要是比较式(1)和式(2)来求解姿态角。但是，存在的一个主要问题是，一个坐标变换矩阵可以用两组不同的姿态角表述，比如，
Figure BDA0002509773460000092
就可用另一组姿态角
Figure BDA0002509773460000093
π-ψ、γ-π表述为
Figure BDA0002509773460000094
因此，根据
Figure BDA0002509773460000095
求解的姿态角
Figure BDA0002509773460000096
ψ、γ不唯一。为此，都把ψ限定在(-90°，+90°)，根据式(2)求解的3个姿态角的主值为
Figure BDA0002509773460000097
由于
Figure BDA0002509773460000098
和γ的取值范围定义在[-180°，+180°],根据式(2)对公式(4)进行分段处理可得到精确值。从以上分析可以看出，正因为把ψ限定在(-90°，+90°)范围内，从而使载体(飞机、导弹等)的运动不能实现全姿态的功能。
比如，飞机在做俯仰角(-180°，+180°)翻滚动作时，设姿态角
Figure BDA0002509773460000099
ψ、γ的初值为0°，飞机的角速度
Figure BDA00025097734600000910
时，三个姿态角在翻滚一圈的变换过程如图3所示。但是，采用四元数解算的三个姿态角如图4所示，可以看出，求解的姿态角ψ在变化过程中不能跨过+90°和-90°，
Figure BDA00025097734600000911
和γ会分别变化180°，与飞行轨迹的理论值之间不吻合。这说明，采用原有的四元数解算姿态角的方法存在缺陷和不足。
本实施例提供一种变轨机动飞机全姿态惯性导航的姿态控制方法，基于与载体固连的捷联式惯性系统实现，所述捷联式惯性系统对应的本体坐标系(b系，为动系)为OX′Y′Z′；描述载体运动姿态角对应的导航坐标系(n系，为定系)为OXYZ；两个坐标系的原点重合。
在解算姿态角时，采用克雷洛夫角描述两个坐标系之间的关系，导航坐标系经过三次转动后到达本体坐标系，具体转动过程为：导航坐标系OXYZ绕OZ轴转动
Figure BDA0002509773460000101
角，到达OLNZ；OLNZ再绕ON轴转动ψ角，到达O X′NM；最后，O X′NM绕O X′轴转动γ角，到达OX′Y′Z′。
所述全姿态惯性导航的姿态角解算方法实现步骤如下：
(1)根据本体坐标系相对导航坐标系的初始偏航角
Figure BDA0002509773460000102
初始俯仰角ψ和初始滚转角γ得到坐标变换矩阵
Figure BDA0002509773460000103
及四元数初值q0；其中，
Figure BDA0002509773460000104
的表达式为
Figure BDA0002509773460000105
Figure BDA0002509773460000106
其中，
Figure BDA0002509773460000107
为本体坐标系OZ′轴的单位矢量，
Figure BDA0002509773460000108
为本体坐标系OY′轴的单位矢量，
Figure BDA0002509773460000109
为本体坐标系OX′轴的单位矢量，n为导航坐标系，b为本体坐标系。
(2)预设采样时间为ΔT，实时获取捷联式惯性系统测量的本体相对导航坐标系的角速度
Figure BDA00025097734600001010
其中，ωx′、ωy′、ωz′为沿本体坐标系OX′Y′Z′三个轴OX′、OY′、OZ′的角速度分量；
(3)当ψ角在[-180°，-90°-ε]、[-90°+ε，90°-ε]或[90°+ε，180°]区间时，根据四元数法得到第一坐标变换矩阵
Figure BDA00025097734600001011
以及第一偏航角
Figure BDA00025097734600001012
第一俯仰角ψ'和第一滚转角γ'；其中，ε为临近±90°的域值。
(3.1)依据步骤(2)的角速度ωx′、ωy′、ωz′计算的瞬时转角为：
Figure BDA0002509773460000111
(3.2)依据步骤(1)的四元数初值q0与步骤(3.1)的Δθ，得到第一四元数q：
Figure BDA0002509773460000112
式中，第一四元数
Figure BDA0002509773460000113
可写为向量表达式
Figure BDA0002509773460000114
其中，Δθ为采样时间ΔT内本体坐标系相对导航坐标系的转角，λ为四元数的标量，ρ1为四元数沿本体坐标系OX′轴的分量，ρ2为四元数沿本体坐标系OY′轴的分量，ρ3为四元数沿本体坐标系OZ′轴的分量。
(3.3)根据步骤(3.2)的第一四元数的各元素λ、ρ1、ρ2、ρ3，得到从导航坐标系至本体坐标系的第一坐标变换矩阵
Figure BDA0002509773460000115
为
Figure BDA0002509773460000116
式中，b为本体坐标系，n为导航坐标系；
(3.4)根据步骤(3.3)的第一坐标变换矩阵
Figure BDA0002509773460000117
得到第一偏航角
Figure BDA0002509773460000118
第一俯仰角ψ'和第一滚转角γ'，解算方法为
(3.4.1)当ψ∈[-180°，-90°-ε]或ψ∈[90°+ε，180°]时，有
Figure BDA0002509773460000119
Figure BDA0002509773460000121
Figure BDA0002509773460000122
(3.4.2)当ψ∈[-90°+ε，90°-ε]时，有
ψ'＝-arcsin(2(ρ1ρ3-λρ2))
Figure BDA0002509773460000123
Figure BDA0002509773460000131
(4)当ψ角在(-90°-ε，-90°+ε)或(90°-ε，90°+ε)区间时，根据克雷洛夫角微分方程得到第二坐标变换矩阵
Figure BDA0002509773460000132
以及第二偏航角
Figure BDA0002509773460000133
第二俯仰角ψ”和第二滚转角γ”；
(4.1)根据步骤(1)的初始偏航角
Figure BDA0002509773460000134
初始俯仰角ψ和初始滚转角γ和步骤(2)的角速度，获得第二偏航角
Figure BDA0002509773460000135
第二俯仰角ψ”和第二滚转角γ”；具体计算方法如下
Figure BDA0002509773460000136
(4.2)根据步骤(4.1)的第二偏航角
Figure BDA0002509773460000137
第二俯仰角ψ”和第二滚转角γ”得到第二坐标变换矩阵
Figure BDA0002509773460000138
Figure BDA0002509773460000139
(4.3)根据步骤(4.1)的第二偏航角
Figure BDA00025097734600001310
第二俯仰角ψ”和第二滚转角γ”得到第二四元数q”
Figure BDA00025097734600001311
(5)将步骤(3)得到的第一偏航角
Figure BDA00025097734600001312
第一俯仰角ψ'和第一滚转角γ'或步骤(4)得到的第二偏航角
Figure BDA00025097734600001313
第二俯仰角ψ”和第二滚转角γ”用于变轨机动飞机的姿态控制，将步骤(3)得到的第一坐标变换矩阵
Figure BDA0002509773460000141
或步骤(4)得到的第二坐标变换矩阵
Figure BDA0002509773460000142
用于惯性导航的速度和位置解算。
上述实施例中，在步骤(1)中初始姿态角
Figure BDA0002509773460000143
ψ、γ由外部主惯导或光学瞄准传递对准给出。
上述实施例中，在步骤(3)中的ε确定原则为
ε＝2×max(ωy′,ωz′)×ΔT
上述实施例中，在步骤(3)中计算姿态角时所用的角速度ωx′、ωy′、ωz′由速率陀螺仪测量得到。
上述实施例中，在步骤(1)中，转动角度
Figure BDA00025097734600001410
ψ、γ的取值范围为-180°～+180°。
为形象说明本发明提供的基于克雷洛夫角奇异条件下的姿态角解算方法，优选的实施例为：
例1：飞机在做俯仰角(-180°，+180°)翻滚动作时，设姿态角
Figure BDA0002509773460000144
ψ、γ的初值为0°，飞机的角速度
Figure BDA0002509773460000145
时，三个姿态角在翻滚一圈的变换过程如图3所示。但是，采用四元数解算的三个姿态角如图4所示，可以看出，求解的姿态角ψ在变化过程中不能跨过+90°和-90°，
Figure BDA0002509773460000148
和γ会分别变化180°，与飞行轨迹的理论值之间不吻合。这说明，采用原有的四元数解算姿态角的方法存在缺陷和不足。而采用本发明给出的方法解算的三个姿态角如图5所示，与图3比较可以看出，二者一致，说明了本发明技术的有效。
例2：飞机在做俯仰角(-180°，+180°)翻滚动作时，设姿态角
Figure BDA0002509773460000149
和γ的初值为0°，ψ的初值为135°，飞机的角速度
Figure BDA0002509773460000146
时，三个姿态角在翻滚一圈的变换过程如图6所示。可以看出，本发明给出的方法可有效覆盖全姿态。
以上所述，仅为本发明一个具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (10)

1.一种变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：将捷联式惯性系统安装在变轨机动飞机上，所述捷联式惯性系统在所述捷联式惯性系统的本体上安装三个正交的陀螺仪和三个正交的加速度计，对应的本体坐标系为OX′Y′Z′，其中，OX′轴表示载体的纵轴，OZ′轴表示载体的横轴，OY′轴与其它两轴组成右手坐标系；建立描述载体运动姿态角对应的导航坐标系为OXYZ，导航坐标系取东北天地理坐标系，其中，OX指东，OY轴指北，OZ轴指天；两个坐标系的原点重合；
所述变轨机动飞机全姿态惯性导航的姿态控制方法包括如下步骤：
(1)根据本体坐标系相对导航坐标系的初始偏航角
Figure FDA0002509773450000014
初始俯仰角ψ和初始滚转角γ得到坐标变换矩阵
Figure FDA0002509773450000015
及四元数初值q0；其中，
Figure FDA0002509773450000016
的表达式为：
Figure FDA0002509773450000011
Figure FDA0002509773450000012
其中，
Figure FDA0002509773450000019
为本体坐标系OZ′轴的单位矢量，
Figure FDA00025097734500000111
为本体坐标系OY′轴的单位矢量，
Figure FDA00025097734500000110
为本体坐标系OX′轴的单位矢量，n为导航坐标系，b为本体坐标系；
(2)预设采样时间为ΔT，实时获取捷联式惯性系统测量的本体相对导航坐标系的角速度
Figure FDA0002509773450000013
其中，ωx′、ωy′、ωz′为沿本体坐标系OX′Y′Z′三个轴OX′、OY′、OZ′的角速度分量；
(3)当ψ角在[-180°，-90°-ε]、[-90°+ε，90°-ε]或[90°+ε，180°]区间时，根据四元数法得到第一坐标变换矩阵
Figure FDA0002509773450000017
以及第一偏航角
Figure FDA0002509773450000018
第一俯仰角ψ'和第一滚转角γ'；其中，ε为临近±90°的域值；
(4)当ψ角在(-90°-ε，-90°+ε)或(90°-ε，90°+ε)区间时，根据克雷洛夫角微分方程得到第二坐标变换矩阵
Figure FDA0002509773450000025
以及第二偏航角
Figure FDA0002509773450000024
第二俯仰角ψ”和第二滚转角γ”；
(5)将步骤(3)得到的第一偏航角
Figure FDA0002509773450000026
第一俯仰角ψ'和第一滚转角γ'或步骤(4)得到的第二偏航角
Figure FDA0002509773450000027
第二俯仰角ψ”和第二滚转角γ”用于变轨机动飞机的姿态控制，将步骤(3)得到的第一坐标变换矩阵
Figure FDA0002509773450000028
或步骤(4)得到的第二坐标变换矩阵
Figure FDA0002509773450000029
用于惯性导航的速度和位置解算。
2.根据权利要求1所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(3)中，当ψ角在[-180°，-90°-ε]、[-90°+ε，90°-ε]或[90°+ε，180°]区间时，根据四元数法得到第一坐标变换矩阵
Figure FDA00025097734500000211
以及第一偏航角
Figure FDA00025097734500000210
第一俯仰角ψ'和第一滚转角γ'包括如下步骤：
(3.1)依据步骤(2)的角速度ωx′、ωy′、ωz′计算的瞬时转角为：
Figure FDA0002509773450000021
(3.2)依据步骤(1)的四元数初值q0与步骤(3.1)的Δθ，得到第一四元数q：
Figure FDA0002509773450000022
式中，第一四元数
Figure FDA00025097734500000212
可写为向量表达式
Figure FDA0002509773450000023
其中，Δθ为采样时间ΔT内本体坐标系相对导航坐标系的转角，λ为四元数的标量，ρ1为四元数沿本体坐标系OX′轴的分量，ρ2为四元数沿本体坐标系OY′轴的分量，ρ3为四元数沿本体坐标系OZ′轴的分量；
(3.3)根据步骤(3.2)的第一四元数的各元素λ、ρ1、ρ2、ρ3，得到从导航坐标系至本体坐标系的第一坐标变换矩阵
Figure FDA0002509773450000038
为
Figure FDA0002509773450000031
式中，b为本体坐标系，n为导航坐标系；
(3.4)根据步骤(3.3)的第一坐标变换矩阵
Figure FDA0002509773450000034
得到第一偏航角
Figure FDA0002509773450000035
第一俯仰角ψ'和第一滚转角γ'。
3.根据权利要求2所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(3.4)中，根据步骤(3.3)的第一坐标变换矩阵
Figure FDA0002509773450000037
得到第一偏航角
Figure FDA0002509773450000036
第一俯仰角ψ'和第一滚转角γ'包括如下步骤：
(3.4.1)当ψ∈[-180°，-90°-ε]或ψ∈[90°+ε，180°]时，有
Figure FDA0002509773450000032
Figure FDA0002509773450000033
Figure FDA0002509773450000041
(3.4.2)当ψ∈[-90°+ε，90°-ε]时，有
ψ'＝-arcsin(2(ρ1ρ3-λρ2))
Figure FDA0002509773450000042
Figure FDA0002509773450000043
4.根据权利要求3所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(4)中，当ψ角在(-90°-ε，-90°+ε)或(90°-ε，90°+ε)区间时，根据克雷洛夫角微分方程得到第二坐标变换矩阵
Figure FDA00025097734500000512
以及第二偏航角
Figure FDA0002509773450000055
第二俯仰角ψ”和第二滚转角γ”包括如下步骤：
(4.1)根据步骤(1)的初始偏航角
Figure FDA0002509773450000056
初始俯仰角ψ和初始滚转角γ和步骤(2)的角速度，获得第二偏航角
Figure FDA0002509773450000057
第二俯仰角ψ”和第二滚转角γ”；
(4.2)根据步骤(4.1)的第二偏航角
Figure FDA0002509773450000058
第二俯仰角ψ”和第二滚转角γ”得到第二坐标变换矩阵
Figure FDA0002509773450000059
(4.3)根据步骤(4.1)的第二偏航角
Figure FDA00025097734500000510
第二俯仰角ψ”和第二滚转角γ”得到第二四元数q”：
Figure FDA0002509773450000051
5.根据权利要求4所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(4.1)中，第二偏航角
Figure FDA00025097734500000511
第二俯仰角ψ”和第二滚转角γ”的具体计算方法如下：
Figure FDA0002509773450000052
6.根据权利要求5所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(4.2)中，第二坐标变换矩阵
Figure FDA00025097734500000513
为：
Figure FDA0002509773450000053
7.根据权利要求6所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(4.3)中，第二四元数q”为：
Figure FDA0002509773450000054
8.根据权利要求1所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(3)中，ε确定原则为：
ε＝2×max(ωy′,ωz′)×ΔT。
9.根据权利要求1所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(2)中，ωx′、ωy′、ωz′由速率陀螺仪测量得到。
10.根据权利要求1所述的变轨机动飞机全姿态惯性导航的姿态控制方法，其特征在于：在步骤(1)中，
Figure FDA0002509773450000061
ψ、γ的取值范围均为-180°～+180°。
CN202010457154.9A 2020-05-26 2020-05-26 一种变轨机动飞机全姿态惯性导航的姿态控制方法 Active CN111780749B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010457154.9A CN111780749B (zh) 2020-05-26 2020-05-26 一种变轨机动飞机全姿态惯性导航的姿态控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010457154.9A CN111780749B (zh) 2020-05-26 2020-05-26 一种变轨机动飞机全姿态惯性导航的姿态控制方法

Publications (2)

Publication Number Publication Date
CN111780749A true CN111780749A (zh) 2020-10-16
CN111780749B CN111780749B (zh) 2022-06-03

Family

ID=72754232

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010457154.9A Active CN111780749B (zh) 2020-05-26 2020-05-26 一种变轨机动飞机全姿态惯性导航的姿态控制方法

Country Status (1)

Country Link
CN (1) CN111780749B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112484720A (zh) * 2020-11-17 2021-03-12 天津津航计算技术研究所 一种基于捷联惯导的双欧拉全姿态解算方法
CN113418499A (zh) * 2021-05-13 2021-09-21 青岛杰瑞自动化有限公司 一种旋转飞行器滚转角解算方法及系统
CN113447024A (zh) * 2021-06-28 2021-09-28 北京航天控制仪器研究所 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统
CN113447025A (zh) * 2021-06-28 2021-09-28 北京航天控制仪器研究所 基于克雷洛夫角的惯性导航高精度姿态角解算方法和系统
CN113587925A (zh) * 2021-07-16 2021-11-02 湖南航天机电设备与特种材料研究所 一种惯性导航系统及其全姿态导航解算方法与装置
CN115586793A (zh) * 2022-10-10 2023-01-10 西北工业大学 一种高海况条件下全方位转弯机体指向控制方法
CN115655271A (zh) * 2022-12-22 2023-01-31 中国船舶集团有限公司第七〇七研究所 一种动态条件下的大范围姿态角提取方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120166082A1 (en) * 2010-12-22 2012-06-28 Custom Sensors & Technologies, Inc. Method and system for initial quaternion and attitude estimation
CN103674059A (zh) * 2013-11-11 2014-03-26 北京航天控制仪器研究所 一种基于外测速度信息的sins水平姿态误差修正方法
CN105659817B (zh) * 2012-09-21 2014-07-09 北京航天控制仪器研究所 一种捷联惯性系统全姿态初始对准方法
CN107478110A (zh) * 2017-07-28 2017-12-15 北京航天控制仪器研究所 一种基于状态观测器的旋转弹姿态角计算方法
CN107588771A (zh) * 2017-08-28 2018-01-16 北京工业大学 基于李群描述的捷联惯性导航解算方法
CN110132269A (zh) * 2019-06-10 2019-08-16 西北工业大学 一种导弹高精度垂直发射初始姿态获取方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN110702143B (zh) * 2019-10-19 2021-07-30 北京工业大学 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120166082A1 (en) * 2010-12-22 2012-06-28 Custom Sensors & Technologies, Inc. Method and system for initial quaternion and attitude estimation
CN105659817B (zh) * 2012-09-21 2014-07-09 北京航天控制仪器研究所 一种捷联惯性系统全姿态初始对准方法
CN103674059A (zh) * 2013-11-11 2014-03-26 北京航天控制仪器研究所 一种基于外测速度信息的sins水平姿态误差修正方法
CN107478110A (zh) * 2017-07-28 2017-12-15 北京航天控制仪器研究所 一种基于状态观测器的旋转弹姿态角计算方法
CN107588771A (zh) * 2017-08-28 2018-01-16 北京工业大学 基于李群描述的捷联惯性导航解算方法
CN110132269A (zh) * 2019-06-10 2019-08-16 西北工业大学 一种导弹高精度垂直发射初始姿态获取方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
ZHUANGSHENG ZHU 等: "Research on gravity vertical deflection on attitude of position and orientation system and compensation method", 《AEROSPACE SCIENCE AND TECHNOLOGY》 *
刘生炳 等: "捷联惯导系统全姿态初始对准方法", 《导弹与航天运输技术》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112484720A (zh) * 2020-11-17 2021-03-12 天津津航计算技术研究所 一种基于捷联惯导的双欧拉全姿态解算方法
CN113418499A (zh) * 2021-05-13 2021-09-21 青岛杰瑞自动化有限公司 一种旋转飞行器滚转角解算方法及系统
CN113447024A (zh) * 2021-06-28 2021-09-28 北京航天控制仪器研究所 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统
CN113447025A (zh) * 2021-06-28 2021-09-28 北京航天控制仪器研究所 基于克雷洛夫角的惯性导航高精度姿态角解算方法和系统
CN113447024B (zh) * 2021-06-28 2022-07-05 北京航天控制仪器研究所 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统
CN113447025B (zh) * 2021-06-28 2022-07-29 北京航天控制仪器研究所 基于克雷洛夫角的惯性导航高精度姿态角解算方法和系统
CN113587925A (zh) * 2021-07-16 2021-11-02 湖南航天机电设备与特种材料研究所 一种惯性导航系统及其全姿态导航解算方法与装置
CN115586793A (zh) * 2022-10-10 2023-01-10 西北工业大学 一种高海况条件下全方位转弯机体指向控制方法
CN115655271A (zh) * 2022-12-22 2023-01-31 中国船舶集团有限公司第七〇七研究所 一种动态条件下的大范围姿态角提取方法
CN115655271B (zh) * 2022-12-22 2023-03-10 中国船舶集团有限公司第七〇七研究所 一种动态条件下的大范围姿态角提取方法

Also Published As

Publication number Publication date
CN111780749B (zh) 2022-06-03

Similar Documents

Publication Publication Date Title
CN111780749B (zh) 一种变轨机动飞机全姿态惯性导航的姿态控制方法
CN103983254B (zh) 一种新型敏捷卫星机动中成像方法
CN106197425B (zh) 基于卫星姿态角的地面目标点位置的计算方法
CN108287476B (zh) 基于高阶滑模控制和扰动观测器的空间翻滚非合作目标自主交会制导方法
CN105157705B (zh) 一种半捷联雷达导引头视线角速度提取方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN102944241B (zh) 基于多胞型线性微分包含的航天器相对姿态确定方法
CN106249755A (zh) 一种无人机自主导航系统及导航方法
CN113587925B (zh) 一种惯性导航系统及其全姿态导航解算方法与装置
CN113295162A (zh) 基于无人机状态信息的广义因子图融合导航方法
CN117389312B (zh) 一种基于模型的水下航行器抗横滚三维跟踪控制方法
CN103955138B (zh) 一种基于增量式偏流角的动中成像卫星姿态控制方法
CN116105730A (zh) 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法
CN102436437A (zh) 基于角速度的飞行器极限飞行时四元数傅里埃近似输出方法
CN111605735A (zh) 一种航天器三超控制可变包络角动量分析方法
CN113447025B (zh) 基于克雷洛夫角的惯性导航高精度姿态角解算方法和系统
Guan et al. An innovative high accuracy autonomous navigation method for the Mars rovers
CN113447024B (zh) 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统
CN112747743B (zh) 基于弹载红外导引头的惯性视觉组合导航方法及装置
CN110286688B (zh) 一种带有机械臂的水下四旋翼无人机控制方法
CN112902957A (zh) 一种弹载平台导航方法及系统
CN102508819A (zh) 基于角速度的飞行器极限飞行时四元数勒让德近似输出方法
CN102495831A (zh) 基于角速度的飞行器极限飞行时四元数埃米特近似输出方法
CN116012444B (zh) 一种动态像移补偿偏流曲线拟合方法
CN102506866B (zh) 基于角速度的飞行器极限飞行时四元数切比雪夫近似输出方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant