CN113447024B - 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统 - Google Patents

基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统 Download PDF

Info

Publication number
CN113447024B
CN113447024B CN202110721261.2A CN202110721261A CN113447024B CN 113447024 B CN113447024 B CN 113447024B CN 202110721261 A CN202110721261 A CN 202110721261A CN 113447024 B CN113447024 B CN 113447024B
Authority
CN
China
Prior art keywords
coordinate system
angle
updating
navigation
axis
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.)
Active
Application number
CN202110721261.2A
Other languages
English (en)
Other versions
CN113447024A (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.)
Beijign Institute of Aerospace Control Devices
Original Assignee
Beijign Institute of Aerospace Control Devices
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 Beijign Institute of Aerospace Control Devices filed Critical Beijign Institute of Aerospace Control Devices
Priority to CN202110721261.2A priority Critical patent/CN113447024B/zh
Publication of CN113447024A publication Critical patent/CN113447024A/zh
Application granted granted Critical
Publication of CN113447024B publication Critical patent/CN113447024B/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/20—Instruments for performing navigational calculations
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

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

本发明公开了一种基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统，该方法包括：确定扩展克雷洛夫角；计算得到偏航角、俯仰角、滚转角和扩展俯仰角在tk时刻的值；确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度；进行状态转移和积分更新解算，得到积分更新解算结果；根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，以提高惯性导航的精度。本发明在原三个克雷洛夫角的基础上增加一个姿态角扩展为四个克雷洛夫角，以正交安装于捷联式惯性系统本体上的陀螺仪输出的角速率作为输入信息，实现了惯性导航姿态角的实时更新，使姿态角更新过程中不出现奇异值，从而提高了解算精度。

Description

基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统
技术领域
本发明属于航空、航天技术领域，尤其涉及一种基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统。
背景技术
惯性导航广泛应用于导弹、飞机、舰船和兵器等领域，主要作用是实时确定载体相对导航系的位置、速度和姿态信息。捷联式惯性系统与载体直接固连，通过陀螺仪测量角速度并经数学解算后给出三个姿态角的值。
目前，确定姿态信息的方法有方向余弦运动学方程、欧拉-克雷洛夫角运动学方程以及四元数运动学方程。方向余弦运动学方程的缺点是变换矩阵的参数为9个、联系式为6个，计算量较大，在工程中应用较少。而采用四元数求解的坐标变换矩阵唯一，但由坐标变换矩阵求解姿态角时不存在唯一解。
相对而言，欧拉-克雷洛夫角运动学方程只有3个，但在本技术领域《惯性器件(上)》(中国宇航出版社)第46页中，认为用欧拉-克雷洛夫角描述的运动学方程存在奇点，方程会退化。但是，在专利(202010333184.9)中提出了一种基于克雷洛夫角的姿态解算方法，可以实现载体的全姿态运动描述。但面临的问题是，当俯仰角＝90°时，进行离散化处理时姿态解算的误差较大，从而导致解算的速度和位置误差较大。为此，需要研究一种新型的基于克雷洛夫角的高精度姿态角解算方法，以提高惯性导航精度。
发明内容
本发明的技术解决问题：克服现有技术的不足，提供一种基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统，在原三个克雷洛夫角的基础上增加一个姿态角扩展为四个克雷洛夫角，以正交安装于捷联式惯性系统本体上的陀螺仪输出的角速率作为基于克雷洛夫运动学方程的输入信息，实现惯性导航姿态角的实时更新，使姿态角更新过程中不出现奇异值，从而提高了解算精度，保证了本体坐标系相对导航坐标系的稳定性。
为了解决上述技术问题，本发明公开了一种基于扩展克雷洛夫角的惯性导航姿态角解算方法，包括：
确定扩展克雷洛夫角(
Figure BDA0003136914530000021
ψ,γ,ξ)；其中，扩展克雷洛夫角(
Figure BDA0003136914530000022
ψ,γ,ξ)用于描述导航坐标系与本体坐标系之间的关系，导航坐标系经过四次转动后到达本体坐标系，四次转动对应的转动角分别记作偏航角
Figure BDA0003136914530000023
俯仰角ψ、滚转角γ和扩展俯仰角ξ；
计算得到偏航角
Figure BDA0003136914530000024
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk时刻的值，分别记作：
Figure BDA0003136914530000025
ψk、γk和ξk；
确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure BDA0003136914530000026
根据确定的
Figure BDA0003136914530000027
ψk、γk、ξk和
Figure BDA0003136914530000028
进行状态转移和积分更新解算，得到积分更新解算结果；
根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，以提高惯性导航的精度。
在上述基于扩展克雷洛夫角的惯性导航姿态角解算方法中，
捷联式惯性系统对应的本体坐标系记作O-X′Y′Z′，为动系；其中，OX′轴表示载体的纵轴，OZ′轴表示载体的横纵，OY′轴与OX′轴和OZ′轴组成右手坐标系；
描述载体运动姿态角对应的导航坐标系记作O-XYZ，为定系；其中，初始时刻，OX轴指东，OY轴指北，OZ轴指天；
本体坐标系O-X′Y′Z′与导航坐标系O-XYZ的原点重合，均以捷联式惯性系统的本体的几何中心为原点。
在上述基于扩展克雷洛夫角的惯性导航姿态角解算方法中，导航坐标系到本体坐标系的转动过程如下：
导航坐标系O-XYZ绕OZ轴转动
Figure BDA0003136914530000029
到达坐标系O-LNZ；
坐标系O-LNZ绕ON轴转动ψ，到达坐标系O-QNM；
坐标系O-QNM绕OQ轴转动γ，到达坐标系O-QY′P；
坐标系O-QY′P绕OY′轴转动ξ，到达本体坐标系O-X′Y′Z′。
在上述基于扩展克雷洛夫角的惯性导航姿态角解算方法中，确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure BDA0003136914530000031
包括：
在转动过程中，获取tk时刻陀螺仪输出的角速度：
Figure BDA0003136914530000032
其中，陀螺仪安装在捷联式惯性系统本体上；
Figure BDA0003136914530000033
和
Figure BDA0003136914530000034
为tk时刻本体坐标系的三轴角速度分量；
则，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure BDA0003136914530000035
为：
Figure BDA0003136914530000036
在上述基于扩展克雷洛夫角的惯性导航姿态角解算方法中，根据确定的
Figure BDA0003136914530000037
ψk、γk、ξk和
Figure BDA0003136914530000038
进行状态转移和积分更新解算，得到积分更新解算结果，包括：
当
Figure BDA0003136914530000039
时，有：
对
Figure BDA00031369145300000310
ψk、γk和ξk进行状态转移，得到一组新的姿态角
Figure BDA00031369145300000311
ψ′k、γ′k和ξ′k：
Figure BDA00031369145300000312
以得到的新的姿态角
Figure BDA00031369145300000313
ψ′k、γ′k和ξ′k作为初值，进行积分更新解算，得到偏航角
Figure BDA00031369145300000314
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk+1时刻的值，分别记作：
Figure BDA00031369145300000315
ψk+1、γk+1和ξk+1：
Figure BDA00031369145300000316
Figure BDA0003136914530000041
其中，tk+1＝tk+ΔT，ΔT表示采样时间。
在上述基于扩展克雷洛夫角的惯性导航姿态角解算方法中，根据确定的
Figure BDA0003136914530000042
ψk、γk、ξk和
Figure BDA0003136914530000043
进行状态转移和积分更新解算，得到积分更新解算结果，包括：
当
Figure BDA0003136914530000044
或
Figure BDA0003136914530000045
时，有：
对
Figure BDA0003136914530000046
ψk、γk和ξk进行状态转移，得到一组新的姿态角
Figure BDA0003136914530000047
ψ′k、γ′k和ξ′k：
Figure BDA0003136914530000048
以得到的新的姿态角
Figure BDA0003136914530000049
ψ′k、γ′k和ξ′k作为初值，进行积分更新解算，得到偏航角
Figure BDA00031369145300000410
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk+1时刻的值，分别记作：
Figure BDA00031369145300000411
ψk+1、γk+1和ξk+1：
ψk+1＝ψ′k
Figure BDA00031369145300000412
其中，tk+1＝tk+ΔT，ΔT表示采样时间。
在上述基于扩展克雷洛夫角的惯性导航姿态角解算方法中，偏航角
Figure BDA00031369145300000413
俯仰角ψ、滚转角γ和扩展俯仰角ξ的取值范围均为：-180°～+180°。
在上述基于扩展克雷洛夫角的惯性导航姿态角解算方法中，安装在捷联式惯性系统本体上的陀螺仪为3个单自由度陀螺仪或2个双自由度陀螺仪。
在上述基于扩展克雷洛夫角的惯性导航姿态角解算方法中，根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，包括：
根据
Figure BDA0003136914530000051
ψk+1、γk+1和ξk+1，进行姿态坐标变换矩阵更新，得到更新后的本体坐标系相对导航坐标系的坐标变换矩阵
Figure BDA0003136914530000052
Figure BDA0003136914530000053
将更新后的坐标变换矩阵
Figure BDA0003136914530000054
以及视加速度
Figure BDA0003136914530000055
和重力加速度
Figure BDA0003136914530000056
作为速度微分方程
Figure BDA0003136914530000057
的输入，经积分解算后，得到更新后的速度V；
将更新后的速度V作为位置微分方程
Figure BDA00031369145300000516
的输入，经积分解算后，得到更新后的位置r。
相应的，本发明还公开了一种基于扩展克雷洛夫角的惯性导航姿态角解算系统，包括：
第一确定模块，用于确定扩展克雷洛夫角(
Figure BDA0003136914530000058
ψ,γ,ξ)；其中，扩展克雷洛夫角(
Figure BDA0003136914530000059
ψ,γ,ξ)用于描述导航坐标系与本体坐标系之间的关系，导航坐标系经过四次转动后到达本体坐标系，四次转动对应的转动角分别记作偏航角
Figure BDA00031369145300000510
俯仰角ψ、滚转角γ和扩展俯仰角ξ；
计算模块，用于计算得到偏航角
Figure BDA00031369145300000511
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk时刻的值，分别记作：
Figure BDA00031369145300000512
ψk、γk和ξk；
第二确定模块，用于确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure BDA00031369145300000513
解算模块，用于根据确定的
Figure BDA00031369145300000514
ψk、γk、ξk和
Figure BDA00031369145300000515
进行状态转移和积分更新解算，得到积分更新解算结果；
更新模块，用于根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，以提高惯性导航的精度。
本发明具有以下优点：
(1)本发明公开了一种基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统，相对于3个姿态角描述的姿态角解算，精度提高了1～2个数量级，实现了姿态角的高精度解算。
(2)本发明公开了一种基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统，完全覆盖了4个姿态角在任意象限的情况，有效减小了原有3个姿态角解算方法在俯仰角为±90°时由于离散化造成的量化误差引起的动态误差较大问题。
(3)本发明公开了一种基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统，可直接精确给出本体坐标系相对导航坐标系的姿态角，而四元数只能通过坐标变换矩阵间接给出。
(4)本发明公开了一种基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统，首次给出了基于克雷洛夫角的捷联式惯性系统离散化全姿态解算方法，具有精度高的优点。
附图说明
图1是本发明实施例中一种基于扩展克雷洛夫角的惯性导航姿态角解算方法的步骤流程图；
图2为捷联式惯性系统中以克雷洛夫角描述的本体相对导航坐标系的关系示意图；
图3为采用原3个克雷洛夫角描述的飞机翻滚三圈过程中解算的三个姿态角；
图4为采用原3个克雷洛夫角描述的飞机翻滚三圈过程中解算的三个姿态角误差；
图5为本发明基于4个克雷洛夫角描述的飞机翻滚三圈过程中的四个姿态角；
图6为本发明基于4个克雷洛夫角描述的飞机翻滚三圈过程中的四个姿态角误差；
图7为基于本发明计算的姿态角进行导航解算的飞机运动三维轨迹。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公开的实施方式作进一步详细描述。
如图1，在本实施例中，该基于扩展克雷洛夫角的惯性导航姿态角解算方法，包括：
步骤101，确定扩展克雷洛夫角(
Figure BDA0003136914530000071
ψ,γ,ξ)。
在本实施例中，扩展克雷洛夫角(
Figure BDA0003136914530000072
ψ,γ,ξ)用于描述导航坐标系与本体坐标系之间的关系，导航坐标系经过四次转动后到达本体坐标系，四次转动对应的转动角分别记作偏航角
Figure BDA0003136914530000073
俯仰角ψ、滚转角γ和扩展俯仰角ξ。其中，偏航角
Figure BDA0003136914530000074
俯仰角ψ、滚转角γ和扩展俯仰角ξ的取值范围均为：-180°～+180°。
如图2所示，捷联式惯性系统对应的本体坐标系记作O-X′Y′Z′，为动系；其中，OX′轴表示载体的纵轴，OZ′轴表示载体的横纵，OY′轴与OX′轴和OZ′轴组成右手坐标系。描述载体运动姿态角对应的导航坐标系记作O-XYZ，为定系；其中，初始时刻，OX轴指东，OY轴指北，OZ轴指天。本体坐标系O-X′Y′Z′与导航坐标系O-XYZ的原点重合，均以捷联式惯性系统的本体的几何中心为原点。
优选的，导航坐标系到本体坐标系的转动过程如下：导航坐标系O-XYZ绕OZ轴转动
Figure BDA0003136914530000075
到达坐标系O-LNZ；坐标系O-LNZ绕ON轴转动ψ，到达坐标系O-QNM；坐标系O-QNM绕OQ轴转动γ，到达坐标系O-QY′P；坐标系O-QY′P绕OY′轴转动ξ，到达本体坐标系O-X′Y′Z′。
步骤102，计算得到偏航角
Figure BDA0003136914530000076
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk时刻的值，分别记作：
Figure BDA0003136914530000077
ψk、γk和ξk。
步骤103，确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure BDA0003136914530000081
在本实施例中，在转动过程中，获取tk时刻陀螺仪输出的角速度：
Figure BDA0003136914530000082
则，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure BDA0003136914530000083
为：
Figure BDA0003136914530000084
其中，陀螺仪安装在捷联式惯性系统本体上，且安装在捷联式惯性系统本体上的陀螺仪为3个单自由度陀螺仪或2个双自由度陀螺仪。
Figure BDA0003136914530000085
和
Figure BDA0003136914530000086
为tk时刻本体坐标系的三轴角速度分量。
步骤104，根据确定的
Figure BDA0003136914530000087
ψk、γk、ξk和
Figure BDA0003136914530000088
进行状态转移和积分更新解算，得到积分更新解算结果。
在本实施例中，当
Figure BDA0003136914530000089
时，有：
对
Figure BDA00031369145300000810
ψk、γk和ξk进行状态转移，得到一组新的姿态角
Figure BDA00031369145300000811
ψ′k、γ′k和ξ′k：
Figure BDA00031369145300000812
以得到的新的姿态角
Figure BDA00031369145300000813
ψ′k、γ′k和ξ′k作为初值，进行积分更新解算，得到偏航角
Figure BDA00031369145300000814
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk+1时刻的值，分别记作：
Figure BDA00031369145300000815
ψk+1、γk+1和ξk+1：
Figure BDA00031369145300000816
Figure BDA0003136914530000091
在本实施例中，当
Figure BDA0003136914530000092
或
Figure BDA0003136914530000093
时，有：
对
Figure BDA0003136914530000094
ψk、γk和ξk进行状态转移，得到一组新的姿态角
Figure BDA0003136914530000095
ψ′k、γ′k和ξ′k：
Figure BDA0003136914530000096
以得到的新的姿态角
Figure BDA0003136914530000097
ψ′k、γ′k和ξ′k作为初值，进行积分更新解算，得到偏航角
Figure BDA0003136914530000098
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk+1时刻的值，分别记作：
Figure BDA0003136914530000099
ψk+1、γk+1和ξk+1：
ψk+1＝ψ′k
Figure BDA00031369145300000910
其中，tk+1＝tk+ΔT，ΔT表示采样时间。
步骤105，根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，以提高惯性导航的精度。
在本实施例中，可以根据
Figure BDA00031369145300000911
ψk+1、γk+1和ξk+1，进行姿态坐标变换矩阵更新，得到更新后的本体坐标系相对导航坐标系的坐标变换矩阵
Figure BDA00031369145300000912
Figure BDA0003136914530000101
然后，将更新后的坐标变换矩阵
Figure BDA0003136914530000102
以及视加速度
Figure BDA0003136914530000103
和重力加速度
Figure BDA0003136914530000104
作为速度微分方程
Figure BDA0003136914530000105
的输入，经积分解算后，得到更新后的速度V。进一步的，将更新后的速度V作为位置微分方程
Figure BDA0003136914530000106
的输入，经积分解算后，得到更新后的位置r。
在上述实施例的基础上，下面通过一组示例对比进行说明。
采用捷联式惯性导航解算飞机某次全姿态运动时的姿态角时，如果只采用如下离散化公式进行解算：
Figure BDA0003136914530000107
则，解算得到的解算的1700s～2700s的姿态角如图3所示，图3a、3b、3c依此为滚转角γ、俯仰角ψ、偏航角
Figure BDA0003136914530000108
(单位为“°”)；对应的姿态角误差如图4所示，图4a、4b、4c依此为滚转角误差dγ、俯仰角误差dψ、偏航角误差
Figure BDA00031369145300001012
(单位为“°”)。从图4可以看出，在2700s时刻，滚转角误差dγ接近-0.5°，俯仰角误差dψ接近6.2°，偏航角误差
Figure BDA0003136914530000109
接近-0.2°。上述姿态角误差直接导致速度和位置导航结果偏差较大(位置误差大于3000m)。
采用本发明方法解算得到的1700s～2700s的姿态角如图5所示，图5a、5b、5c、5d依次为滚转角γ、俯仰角ψ、偏航角
Figure BDA00031369145300001013
和扩展俯仰角ξ(单位为“°”)。从5图中可以看出，在1964.66s之前，
Figure BDA00031369145300001010
因此，ψ为0，而
Figure BDA00031369145300001011
γ、ξ按照相应公式进行姿态解算。而在1964.66s时刻，
Figure BDA0003136914530000111
进行状态转移，从初始状态
Figure BDA0003136914530000112
ψ＝0、γ＝-45.0°、ξ＝-13.1°瞬时转移到末端状态
Figure BDA0003136914530000113
ψ＝135.0°、γ＝90.0°、ξ＝-103.2°。在1964.66s之后，
Figure BDA0003136914530000114
因此，
Figure BDA0003136914530000115
为0，而ψ、γ、ξ按照相应公式进行姿态解算。
姿态角误差如图6所示，图6a、6b、6c、6d依次为
滚转角误差dγ、俯仰角误差dψ、偏航角误差
Figure BDA0003136914530000116
和扩展俯仰角误差dξ(单位为“°”)。从图6可以看出，最大角度误差不大于0.002°，具备较高的精度。上述姿态角误差引起的速度和位置导航偏差较小(位置误差小于20m)可忽略。基于该滚转角γ、俯仰角ψ、偏航角
Figure BDA0003136914530000117
和扩展俯仰角ξ导航解算的飞机运动三维轨迹见图7，可以看出，很好的复现出飞机大姿态高机动的运动过程。
上述实施例可以验证本发明的基于扩展克雷洛夫角的惯性导航姿态角解算方法正确，有利于实现全姿态条件下的高精度速度和位置解算。
在上述实施例的基础上，本发明还公开了一种基于扩展克雷洛夫角的惯性导航姿态角解算系统，包括：
第一确定模块，用于确定扩展克雷洛夫角(
Figure BDA0003136914530000118
ψ,γ,ξ)；其中，扩展克雷洛夫角(
Figure BDA0003136914530000119
ψ,γ,ξ)用于描述导航坐标系与本体坐标系之间的关系，导航坐标系经过四次转动后到达本体坐标系，四次转动对应的转动角分别记作偏航角
Figure BDA00031369145300001110
俯仰角ψ、滚转角γ和扩展俯仰角ξ。
计算模块，用于计算得到偏航角
Figure BDA00031369145300001111
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk时刻的值，分别记作：
Figure BDA00031369145300001112
ψk、γk和ξk。
第二确定模块，用于确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure BDA00031369145300001113
解算模块，用于根据确定的
Figure BDA00031369145300001114
ψk、γk、ξk和
Figure BDA00031369145300001115
进行状态转移和积分更新解算，得到积分更新解算结果。
更新模块，用于根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，以提高惯性导航的精度。
对于系统实施例而言，由于其与方法实施例相对应，所以描述的比较简单，相关之处参见方法实施例部分的说明即可。
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (4)

1.一种基于扩展克雷洛夫角的惯性导航姿态角解算方法，其特征在于，包括：
确定扩展克雷洛夫角
Figure FDA0003608452330000011
其中，扩展克雷洛夫角
Figure FDA0003608452330000012
用于描述导航坐标系与本体坐标系之间的关系，导航坐标系经过四次转动后到达本体坐标系，四次转动对应的转动角分别记作偏航角
Figure FDA0003608452330000013
俯仰角ψ、滚转角γ和扩展俯仰角ξ；捷联式惯性系统对应的本体坐标系记作O-X′Y′Z′，为动系，OX′轴表示载体的纵轴，OZ′轴表示载体的横纵，OY′轴与OX′轴和OZ′轴组成右手坐标系；描述载体运动姿态角对应的导航坐标系记作O-XYZ，为定系，初始时刻OX轴指东、OY轴指北、OZ轴指天；本体坐标系O-X′Y′Z′与导航坐标系O-XYZ的原点重合，均以捷联式惯性系统的本体的几何中心为原点；导航坐标系到本体坐标系的转动过程如下：导航坐标系O-XYZ绕OZ轴转动
Figure FDA0003608452330000014
到达坐标系O-LNZ；坐标系O-LNZ绕ON轴转动ψ，到达坐标系O-QNM；坐标系O-QNM绕OQ轴转动γ，到达坐标系O-QY′P；坐标系O-QY′P绕OY′轴转动ξ，到达本体坐标系O-X′Y′Z′；
计算得到偏航角
Figure FDA0003608452330000015
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk时刻的值，分别记作：
Figure FDA0003608452330000016
ψk、γk和ξk；
确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure FDA0003608452330000017
根据确定的
Figure FDA0003608452330000018
ψk、γk、ξk和
Figure FDA0003608452330000019
进行状态转移和积分更新解算，得到积分更新解算结果；
根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，以提高惯性导航的精度；
其中：
确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure FDA00036084523300000110
包括：
在转动过程中，获取tk时刻陀螺仪输出的角速度：
Figure FDA0003608452330000021
其中，陀螺仪安装在捷联式惯性系统本体上；
Figure FDA0003608452330000022
和
Figure FDA0003608452330000023
为tk时刻本体坐标系的三轴角速度分量；
则，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure FDA0003608452330000024
为：
Figure FDA0003608452330000025
根据确定的
Figure FDA0003608452330000026
ψk、γk、ξk和
Figure FDA0003608452330000027
进行状态转移和积分更新解算，得到积分更新解算结果，包括：
当
Figure FDA0003608452330000028
时，有：
对
Figure FDA0003608452330000029
ψk、γk和ξk进行状态转移，得到一组新的姿态角
Figure FDA00036084523300000210
ψ′k、γ′k和ξ′k：
Figure FDA00036084523300000211
以得到的新的姿态角
Figure FDA00036084523300000212
ψ′k、γ′k和ξ′k作为初值，进行积分更新解算，得到偏航角
Figure FDA00036084523300000213
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk+1时刻的值，分别记作：
Figure FDA00036084523300000214
ψk+1、γk+1和ξk+1：
Figure FDA00036084523300000215
Figure FDA00036084523300000216
其中，tk+1＝tk+ΔT，ΔT表示采样时间；
当
Figure FDA00036084523300000217
或
Figure FDA00036084523300000218
时，有：
对
Figure FDA0003608452330000031
ψk、γk和ξk进行状态转移，得到一组新的姿态角
Figure FDA0003608452330000032
ψ′k、γ′k和ξ′k：
Figure FDA0003608452330000033
以得到的新的姿态角
Figure FDA0003608452330000034
ψ′k、γ′k和ξ′k作为初值，进行积分更新解算，得到偏航角
Figure FDA0003608452330000035
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk+1时刻的值，分别记作：
Figure FDA0003608452330000036
ψk+1、γk+1和ξk+1：
ψk+1＝ψ′k
Figure FDA0003608452330000037
根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，包括：
根据
Figure FDA0003608452330000038
ψk+1、γk+1和ξk+1，进行姿态坐标变换矩阵更新，得到更新后的本体坐标系相对导航坐标系的坐标变换矩阵
Figure FDA0003608452330000039
Figure FDA00036084523300000310
将更新后的坐标变换矩阵
Figure FDA00036084523300000311
以及视加速度
Figure FDA00036084523300000315
和重力加速度
Figure FDA00036084523300000312
作为速度微分方程
Figure FDA00036084523300000313
的输入，经积分解算后，得到更新后的速度V；
将更新后的速度V作为位置微分方程
Figure FDA00036084523300000314
的输入，经积分解算后，得到更新后的位置r。
2.根据权利要求1所述的基于扩展克雷洛夫角的惯性导航姿态角解算方法，其特征在于，偏航角
Figure FDA0003608452330000041
俯仰角ψ、滚转角γ和扩展俯仰角ξ的取值范围均为：-180°～+180°。
3.根据权利要求1所述的基于扩展克雷洛夫角的惯性导航姿态角解算方法，其特征在于，安装在捷联式惯性系统本体上的陀螺仪为3个单自由度陀螺仪或2个双自由度陀螺仪。
4.一种基于扩展克雷洛夫角的惯性导航姿态角解算系统，其特征在于，包括：
第一确定模块，用于确定扩展克雷洛夫角
Figure FDA0003608452330000042
其中，扩展克雷洛夫角
Figure FDA0003608452330000043
用于描述导航坐标系与本体坐标系之间的关系，导航坐标系经过四次转动后到达本体坐标系，四次转动对应的转动角分别记作偏航角
Figure FDA0003608452330000044
俯仰角ψ、滚转角γ和扩展俯仰角ξ；捷联式惯性系统对应的本体坐标系记作O-X′Y′Z′，为动系，OX′轴表示载体的纵轴，OZ′轴表示载体的横纵，OY′轴与OX′轴和OZ′轴组成右手坐标系；描述载体运动姿态角对应的导航坐标系记作O-XYZ，为定系，初始时刻OX轴指东、OY轴指北、OZ轴指天；本体坐标系O-X′Y′Z′与导航坐标系O-XYZ的原点重合，均以捷联式惯性系统的本体的几何中心为原点；导航坐标系到本体坐标系的转动过程如下：导航坐标系O-XYZ绕OZ轴转动
Figure FDA0003608452330000045
到达坐标系O-LNZ；坐标系O-LNZ绕ON轴转动ψ，到达坐标系O-QNM；坐标系O-QNM绕OQ轴转动γ，到达坐标系O-QY′P；坐标系O-QY′P绕OY′轴转动ξ，到达本体坐标系O-X′Y′Z′；
计算模块，用于计算得到偏航角
Figure FDA0003608452330000046
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk时刻的值，分别记作：
Figure FDA0003608452330000047
ψk、γk和ξk；
第二确定模块，用于确定转动过程中，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure FDA0003608452330000048
解算模块，用于根据确定的
Figure FDA0003608452330000051
ψk、γk、ξk和
Figure FDA0003608452330000052
进行状态转移和积分更新解算，得到积分更新解算结果；
更新模块，用于根据积分更新解算结果，进行坐标变换矩阵更新，并支撑速度更新和位置更新，以提高惯性导航的精度；
其中：
第二确定模块，具体用于：
在转动过程中，获取tk时刻陀螺仪输出的角速度：
Figure FDA0003608452330000053
其中，陀螺仪安装在捷联式惯性系统本体上；
Figure FDA0003608452330000054
和
Figure FDA0003608452330000055
为tk时刻本体坐标系的三轴角速度分量；
则，tk时刻时本体坐标系相对于导航坐标系的角速度
Figure FDA0003608452330000056
为：
Figure FDA0003608452330000057
解算模块，具体用于：
当
Figure FDA0003608452330000058
时，有：
对
Figure FDA00036084523300000515
ψk、γk和ξk进行状态转移，得到一组新的姿态角
Figure FDA0003608452330000059
ψ′k、γ′k和ξ′k：
Figure FDA00036084523300000510
以得到的新的姿态角
Figure FDA00036084523300000511
ψ′k、γ′k和ξ′k作为初值，进行积分更新解算，得到偏航角
Figure FDA00036084523300000512
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk+1时刻的值，分别记作：
Figure FDA00036084523300000513
ψk+1、γk+1和ξk+1：
Figure FDA00036084523300000514
Figure FDA0003608452330000061
其中，tk+1＝tk+ΔT，ΔT表示采样时间；
当
Figure FDA0003608452330000062
或
Figure FDA0003608452330000063
时，有：
对
Figure FDA0003608452330000064
ψk、γk和ξk进行状态转移，得到一组新的姿态角
Figure FDA0003608452330000065
ψ′k、γ′k和ξ′k：
Figure FDA0003608452330000066
以得到的新的姿态角
Figure FDA0003608452330000067
ψ′k、γ′k和ξ′k作为初值，进行积分更新解算，得到偏航角
Figure FDA0003608452330000068
俯仰角ψ、滚转角γ和扩展俯仰角ξ在tk+1时刻的值，分别记作：
Figure FDA0003608452330000069
ψk+1、γk+1和ξk+1：
ψk+1＝ψ′k
Figure FDA00036084523300000610
更新模块，具体用于：
根据
Figure FDA00036084523300000611
ψk+1、γk+1和ξk+1，进行姿态坐标变换矩阵更新，得到更新后的本体坐标系相对导航坐标系的坐标变换矩阵
Figure FDA00036084523300000612
Figure FDA00036084523300000613
将更新后的坐标变换矩阵
Figure FDA0003608452330000071
以及视加速度
Figure FDA0003608452330000072
和重力加速度
Figure FDA0003608452330000073
作为速度微分方程
Figure FDA0003608452330000074
的输入，经积分解算后，得到更新后的速度V；
将更新后的速度V作为位置微分方程
Figure FDA0003608452330000075
的输入，经积分解算后，得到更新后的位置r。
CN202110721261.2A 2021-06-28 2021-06-28 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统 Active CN113447024B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110721261.2A CN113447024B (zh) 2021-06-28 2021-06-28 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110721261.2A CN113447024B (zh) 2021-06-28 2021-06-28 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统

Publications (2)

Publication Number Publication Date
CN113447024A CN113447024A (zh) 2021-09-28
CN113447024B true CN113447024B (zh) 2022-07-05

Family

ID=77813514

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110721261.2A Active CN113447024B (zh) 2021-06-28 2021-06-28 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统

Country Status (1)

Country Link
CN (1) CN113447024B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115060256B (zh) * 2022-07-05 2022-11-08 西北工业大学 一种基于发射坐标系的制导炮弹空中姿态辨识方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4038527A (en) * 1975-10-21 1977-07-26 The Singer Company Simplified strapped down inertial navigation utilizing bang-bang gyro torquing
CN105659817B (zh) * 2012-09-21 2014-07-09 北京航天控制仪器研究所 一种捷联惯性系统全姿态初始对准方法
CN104457446A (zh) * 2014-11-28 2015-03-25 北京航天控制仪器研究所 一种自旋制导炮弹的空中自对准方法
CN107478110A (zh) * 2017-07-28 2017-12-15 北京航天控制仪器研究所 一种基于状态观测器的旋转弹姿态角计算方法
CN107588771A (zh) * 2017-08-28 2018-01-16 北京工业大学 基于李群描述的捷联惯性导航解算方法
WO2020124678A1 (zh) * 2018-12-19 2020-06-25 上海交通大学 一种基于函数迭代积分的惯性导航解算方法及系统
CN111623768A (zh) * 2020-04-24 2020-09-04 北京航天控制仪器研究所 一种基于克雷洛夫角奇异条件下的姿态角解算方法
CN111780749A (zh) * 2020-05-26 2020-10-16 北京航天控制仪器研究所 一种变轨机动飞机全姿态惯性导航的姿态控制方法
RU2749152C1 (ru) * 2020-06-19 2021-06-07 Акционерное общество Московский научно-производственный комплекс "Авионика" имени О.В. Успенского (АО МНПК "Авионика") Адаптивный корректор углов ориентации для БИНС

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4038527A (en) * 1975-10-21 1977-07-26 The Singer Company Simplified strapped down inertial navigation utilizing bang-bang gyro torquing
CN105659817B (zh) * 2012-09-21 2014-07-09 北京航天控制仪器研究所 一种捷联惯性系统全姿态初始对准方法
CN104457446A (zh) * 2014-11-28 2015-03-25 北京航天控制仪器研究所 一种自旋制导炮弹的空中自对准方法
CN107478110A (zh) * 2017-07-28 2017-12-15 北京航天控制仪器研究所 一种基于状态观测器的旋转弹姿态角计算方法
CN107588771A (zh) * 2017-08-28 2018-01-16 北京工业大学 基于李群描述的捷联惯性导航解算方法
WO2020124678A1 (zh) * 2018-12-19 2020-06-25 上海交通大学 一种基于函数迭代积分的惯性导航解算方法及系统
CN111623768A (zh) * 2020-04-24 2020-09-04 北京航天控制仪器研究所 一种基于克雷洛夫角奇异条件下的姿态角解算方法
CN111780749A (zh) * 2020-05-26 2020-10-16 北京航天控制仪器研究所 一种变轨机动飞机全姿态惯性导航的姿态控制方法
RU2749152C1 (ru) * 2020-06-19 2021-06-07 Акционерное общество Московский научно-производственный комплекс "Авионика" имени О.В. Успенского (АО МНПК "Авионика") Адаптивный корректор углов ориентации для БИНС

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
刘生炳等.捷联惯导系统全姿态初始对准方法.《导弹与航天运载技术》.2013,(第06期), *
范奎武.用四元数描述飞行器姿态时的几个基本问题.《航天控制》.2012,(第04期), *

Also Published As

Publication number Publication date
CN113447024A (zh) 2021-09-28

Similar Documents

Publication Publication Date Title
CN109813311B (zh) 一种无人机编队协同导航方法
CN107607113B (zh) 一种两轴姿态倾角测量方法
CN111780749B (zh) 一种变轨机动飞机全姿态惯性导航的姿态控制方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN113587925B (zh) 一种惯性导航系统及其全姿态导航解算方法与装置
CN109059914B (zh) 一种基于gps和最小二乘滤波的炮弹滚转角估计方法
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN107270938A (zh) 基于泰勒级数拟合的单轴旋转惯导系统姿态解调方法
CN113447024B (zh) 基于扩展克雷洛夫角的惯性导航姿态角解算方法和系统
CN112902956A (zh) 一种手持式gnss/mems-ins接收机航向初值获取方法、电子设备、存储介质
CN114216456A (zh) 一种基于imu与机器人本体参数融合的姿态测量方法
CN113447025B (zh) 基于克雷洛夫角的惯性导航高精度姿态角解算方法和系统
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN109029499B (zh) 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN112729332B (zh) 一种基于旋转调制的对准方法
CN112284412B (zh) 一种避免欧拉转换奇异导致精度下降的地面静态对准方法
Hong et al. Application of EKF for missile attitude estimation based on “SINS/CNS” integrated guidance system
CN111238532A (zh) 一种适用于晃动基座环境的惯性测量单元标定方法
CN114061575A (zh) 大失准角条件下的导弹姿态角精对准方法及系统
CN110260862B (zh) 一种基于捷联惯导系统的旋翼直升机载导航装置
CN112033439A (zh) 一种摇摆基座地球系下重力加速度矢量无纬度构建方法
CN110967007B (zh) 一种适用于稳态航行可节省两轴捷联陀螺的惯导解算方法
CN116642485A (zh) 惯性导航6参数坐标变换矩阵解算方法
CN116182900B (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