CN113587925B - 一种惯性导航***及其全姿态导航解算方法与装置 - Google Patents

一种惯性导航***及其全姿态导航解算方法与装置 Download PDF

Info

Publication number
CN113587925B
CN113587925B CN202110808144.XA CN202110808144A CN113587925B CN 113587925 B CN113587925 B CN 113587925B CN 202110808144 A CN202110808144 A CN 202110808144A CN 113587925 B CN113587925 B CN 113587925B
Authority
CN
China
Prior art keywords
carrier
conversion matrix
coordinate system
angle
navigation
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
CN202110808144.XA
Other languages
English (en)
Other versions
CN113587925A (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.)
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Original Assignee
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
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 Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials filed Critical Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Priority to CN202110808144.XA priority Critical patent/CN113587925B/zh
Publication of CN113587925A publication Critical patent/CN113587925A/zh
Application granted granted Critical
Publication of CN113587925B publication Critical patent/CN113587925B/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
    • 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

本发明公开了一种惯性导航***及其全姿态导航解算方法与装置，包括初始对准，计算初始姿态转换矩阵；对载体的速度、位置和姿态进行更新，得到更新后的姿态转换矩阵；在更新后的姿态转换矩阵中第3行第2列元素的绝对值大于设定阈值时采用全姿态导航解算俯仰角、滚动角和方位角，在更新后的姿态转换矩阵中第3行第2列元素的绝对值小于等于设定阈值时采用常规方法导航解算俯仰角、滚动角和方位角；通过三轴加速度计增量和陀螺增量进行全姿态下载体姿态角、运动速度、运动位移的计算，且能够对俯仰角绝对值大于89.2°时的方位角进行跟踪，解决了在俯仰角绝对值大于89.2°时的方位角姿态解算不准确的问题。

Description

一种惯性导航***及其全姿态导航解算方法与装置
技术领域
本发明属于惯性导航技术领域，尤其涉及一种惯性导航***及其全姿态导航解算方法与装置。
背景技术
在惯性导航姿态解算过程中，当俯仰角接近±90°(例如俯仰角绝对值大于89.2°)时，方位角和滚动角的解算会出现耦合，导致直接解算出的方位角和滚动角与实际情况不符。虽然此时速度和位移导航结果是正常的，但在一些需要监视导航姿态的***中，就会由于解算出的方位角和滚动角不符合实际而造成姿态预警***误报警。
发明内容
本发明的目的在于提供一种惯性导航***及其全姿态导航解算方法与装置，以解决当俯仰角接近±90°时，由于耦合导致直接解算出的方位角和滚动角与实际情况不符的问题。
本发明是通过如下的技术方案来解决上述技术问题的：一种惯性导航***的全姿态导航解算方法，包括以下步骤：
步骤1：对搭载惯性导航***的载体速度、位置和姿态进行初始对准，计算初始姿态转换矩阵；
步骤2：根据所述步骤1中的初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新；
步骤3：根据所述步骤1中的初始姿态转换矩阵、所述步骤2中更新后的速度和位置对载体在当前时刻k的姿态进行更新，得到更新后的姿态转换矩阵；
步骤4：判断所述步骤3中更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值，如果是，则按照如下公式解算载体在当前时刻k的俯仰角、滚动角和方位角，具体公式为：
Figure GDA0004255606470000011
Figure GDA0004255606470000012
Figure GDA0004255606470000013
其中，θk为当前时刻k的俯仰角，γk为当前时刻k的滚动角，
Figure GDA0004255606470000014
为当前时刻k的方位角ψk的微分，/>
Figure GDA0004255606470000015
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000016
中第3行第2列的元素，/>
Figure GDA0004255606470000017
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000021
中第1行第3列的元素，/>
Figure GDA0004255606470000022
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000023
中第1行第1列的元素，/>
Figure GDA0004255606470000024
为Y轴陀螺在当前时刻k的角增量输出，/>
Figure GDA0004255606470000025
为Z轴陀螺在当前时刻k的角增量输出；
否则，按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角；
步骤5：重复步骤2～4，解算载体在下一时刻k+1的俯仰角、滚动角和方位角，直到惯性导航结束。
进一步地，所述步骤1中，初始姿态转换矩阵的计算过程为：
计算当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影，具体为：
gn＝[0 0 g]T
Figure GDA0004255606470000026
其中，gn为当地重力加速度g在导航坐标系n上的投影，L为载体当前所在位置的纬度，
Figure GDA0004255606470000027
为地球自转角速度ωie在导航坐标系n上的投影；
计算当地重力加速度g和地球自转角速度ωie在载体坐标系b上的投影；
根据当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系计算初始姿态转换矩阵，所述初始姿态转换矩阵的表达式为：
Figure GDA0004255606470000028
其中，
Figure GDA0004255606470000029
为初始姿态转换矩阵，gb为当地重力加速度g在载体坐标系b上的投影，
Figure GDA00042556064700000210
为地球自转角速度ωie在载体坐标系b上的投影。
进一步地，所述步骤2中，更新后载体的速度微分方程为：
Figure GDA00042556064700000211
Figure GDA00042556064700000212
Figure GDA00042556064700000213
Figure GDA0004255606470000031
其中，R为地球半径，
Figure GDA0004255606470000032
和/>
Figure GDA0004255606470000033
分别为东向速度、北向速度和天向速度的微分，VE(k)、VN(k)和VU(k)分别为载体在当前时刻k的东向速度、北向速度和天向速度，L(k-1)和h(k-1)分别为载体在上一时刻k-1的纬度和高度，fE(k)、fN(k)和fU(k)分别为载体在当前时刻k的东向比力、北向比力和天向比力，ωie为地球自转角速度，g为当地重力加速度；ax(k)、ay(k)、az(k)分别为当前时刻k时三轴加速度计X向、Y向、Z向的输出；/>
Figure GDA0004255606470000034
为当前时刻的姿态转换矩阵；
更新后载体的位置微分方程为：
Figure GDA0004255606470000035
Figure GDA0004255606470000036
Figure GDA0004255606470000037
其中，
Figure GDA0004255606470000038
为载体在当前时刻k的经度λ(k)的微分，/>
Figure GDA0004255606470000039
和/>
Figure GDA00042556064700000310
分别为载体在当前时刻k的纬度L(k)和高度h(k)的微分。
进一步地，所述步骤3中，更新后的姿态转换矩阵微分方程为：
Figure GDA00042556064700000311
Figure GDA00042556064700000312
Figure GDA00042556064700000313
Figure GDA00042556064700000314
其中，
Figure GDA00042556064700000315
为载体坐标系b相对于导航坐标系n在当前时刻k的姿态转换矩阵，/>
Figure GDA00042556064700000316
为姿态转换矩阵/>
Figure GDA00042556064700000317
的微分，/>
Figure GDA00042556064700000318
为当前时刻k载体坐标系b相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量，/>
Figure GDA00042556064700000319
为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在导航坐标系n上的投影矢量，/>
Figure GDA0004255606470000041
为投影矢量/>
Figure GDA0004255606470000042
的反对称矩阵，/>
Figure GDA0004255606470000043
为当前时刻k地球自转角速度ωie在导航坐标系n上的投影，/>
Figure GDA0004255606470000044
为当前时刻k导航坐标系n相对于地理系e输出的角速度在导航坐标系n上的投影矢量，L(k-1)和h(k-1)分别为载体在上一时刻k-1的纬度和高度，VE(k-1)、VN(k-1)和VU(k-1)分别为载体在上一时刻k-1的东向速度、北向速度和天向速度，RM、RN为地球曲率半径。
进一步地，所述步骤4中，当当前时刻k的姿态转换矩阵中第3行第2列元素的绝对值大于设定阈值，且俯仰角的绝对值大于89.2°时，当前时刻k的方位角的计算公式为：
Figure GDA0004255606470000045
其中，ψk为步骤4解算出的当前时刻k的方位角，ψ'k为俯仰角的绝对值大于89.2°时当前时刻k的方位角，az(k)为Z轴加速计在当前时刻k的输出，az(k-1)为Z轴加速计在上一时刻k-1的输出。
进一步地，所述步骤4中，按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角的公式分别为：
Figure GDA0004255606470000046
Figure GDA0004255606470000047
Figure GDA0004255606470000048
其中，
Figure GDA0004255606470000049
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700000410
中第3行第1列的元素，/>
Figure GDA00042556064700000411
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700000412
中第3行第3列的元素，/>
Figure GDA00042556064700000413
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700000414
中第1行第2列的元素，/>
Figure GDA00042556064700000415
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700000416
中第2行第2列的元素。
本发明还提供一种惯性导航***的全姿态导航解算装置，包括：
初始对准单元，用于对搭载惯性导航***的载体速度、位置和姿态进行初始对准，计算初始姿态转换矩阵；
速度位置更新单元，用于根据所述初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新；
姿态更新单元，用于根据所述初始姿态转换矩阵、所述更新后的速度和位置对载体在当前时刻k的姿态进行更新，得到更新后的姿态转换矩阵；
解算单元，用于判断更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值，如果是，则按照如下公式解算载体在当前时刻k的俯仰角、滚动角和方位角，具体公式为：
Figure GDA0004255606470000051
Figure GDA0004255606470000052
Figure GDA0004255606470000053
其中，θk为当前时刻k的俯仰角，γk为当前时刻k的滚动角，
Figure GDA0004255606470000054
为当前时刻k的方位角ψk的微分，/>
Figure GDA0004255606470000055
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000056
中第3行第2列的元素，/>
Figure GDA0004255606470000057
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000058
中第1行第3列的元素，/>
Figure GDA0004255606470000059
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700000510
中第1行第1列的元素，/>
Figure GDA00042556064700000511
为Y轴陀螺在当前时刻k的角增量输出，/>
Figure GDA00042556064700000512
为Z轴陀螺在当前时刻k的角增量输出；
否则，按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角。
本发明还提供一种惯性导航***，包括如上所述的全姿态导航解算装置。
有益效果
与现有技术相比，本发明的优点在于：
本发明所提供的一种惯性导航***及其全姿态导航解算方法与装置，通过三轴加速度计增量和陀螺增量进行全姿态下载体惯性导航算法下姿态角、运动速度、运动位移的计算，且能够对俯仰角绝对值大于89.2°时的方位角进行跟踪，避开了传统姿态解算中，俯仰角绝对值接近90°时滚动角解算公式的分母趋近0而导致解算误差过大，从而造成姿态角(或方位角)解算与实际情况不符的问题；在俯仰角绝对值大于89.2°时依靠实际陀螺速率输出解算俯仰角，不会发生误报警现象，可以解决在俯仰角绝对值大于89.2°时的方位角姿态解算不准确的问题。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一个实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例中全姿态导航解算方法流程图；
图2是本发明实施例中全姿态导航解算装置软件界面图。
具体实施方式
下面结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面以具体地实施例对本申请的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
如图1所示，本实施例所提供的一种惯性导航***的全姿态导航解算方法，包括以下步骤：
步骤1：对搭载惯性导航***的载体速度、位置和姿态进行初始对准，计算初始姿态转换矩阵。
输入三轴加速度计增量数据和陀螺增量数据，数据格式要求以6列形式保存的纯数据文本，第一至三列分别为XYZ轴向的加速度增量数据，第四至六列分别为XYZ轴向的陀螺增量数据；输入当地重力加速度g，输入当地经度λ(单位为°)，输入当地纬度L(单位为°)；输入导航开始时间，一般默认为300s，前300s必须为静态数据，用于初始对准；导航解算结果从第301s开始输出。
利用载体运动前300s静态数据进行初始对准，空间向量α在导航坐标系n和载体坐标系b之间的坐标转换关系为：
Figure GDA0004255606470000061
其中，αn、αb分别为空间向量α在导航坐标系n、载体坐标系b的坐标表示，
Figure GDA0004255606470000062
为载体坐标系b到导航坐标系n的姿态转换矩阵。
本实施例中，利用当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系来计算初始姿态转换矩阵，具体计算过程为：
1.1计算当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影，具体为：
gn＝[0 0 g]T (2)
Figure GDA0004255606470000063
其中，gn为当地重力加速度g在导航坐标系n上的投影，L为载体当前所在位置的纬度，
Figure GDA0004255606470000071
为地球自转角速度ωie在导航坐标系n上的投影。
1.2计算当地重力加速度g和地球自转角速度ωie在载体坐标系b上的投影gb和
Figure GDA0004255606470000072
其中，gb为300s静态数据中三轴加速度计输出的均值，/>
Figure GDA0004255606470000073
为300s静态数据中陀螺仪输出的均值。
步骤1.1与步骤1.2无先后顺序，可以同时进行。
1.3根据当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系(即式(1))计算初始姿态转换矩阵，初始姿态转换矩阵的表达式为：
Figure GDA0004255606470000074
其中，
Figure GDA0004255606470000075
为初始姿态转换矩阵，gb为当地重力加速度g在载体坐标系b上的投影，
Figure GDA0004255606470000076
为地球自转角速度ωie在载体坐标系b上的投影。
步骤2：根据步骤1中的初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新。
得到初始姿态转换矩阵后，可以更新任意时刻的速度和位置。本实施例中，导航坐标系为东北天坐标系，更新后载体在当前时刻k的速度微分方程为：
Figure GDA0004255606470000077
其中，R为地球半径，
Figure GDA0004255606470000078
和/>
Figure GDA0004255606470000079
分别为东向速度VE(k)、北向速度VN(k)和天向速度VU(k)的微分，VE(k)、VN(k)和VU(k)分别为载体在当前时刻k的东向速度、北向速度和天向速度，L(k-1)和h(k-1)分别为载体在上一时刻k-1的纬度和高度，fE(k)、fN(k)和fU(k)分别为载体在当前时刻k的东向比力、北向比力和天向比力，ωie为地球自转角速度，g为当地重力加速度。
当前时刻k的东向比力、北向比力和天向比力由三轴加速度计的输出左乘姿态转换矩阵求得，具体为：
Figure GDA0004255606470000081
其中，ax(k)、ay(k)、az(k)分别为当前时刻k时三轴加速度计X向、Y向、Z向的输出；
Figure GDA0004255606470000082
为当前时刻的姿态转换矩阵。
解算速度微分方程式(5)即可求得载体在当前时刻k的东向速度VE(k)、北向速度VN(k)和天向速度VU(k)。当前时刻k的位置更新是基于速度更新的，具体为：
Figure GDA0004255606470000083
其中，
Figure GDA0004255606470000084
为载体在当前时刻k的经度λ(k)的微分，/>
Figure GDA0004255606470000085
和/>
Figure GDA0004255606470000086
分别为载体在当前时刻k的纬度L(k)和高度h(k)的微分。解算位置微分方程式(7)即可求得载体在当前时刻k的经度λ(k)、纬度L(k)和高度h(k)。
步骤3：根据步骤1中的初始姿态转换矩阵、步骤2中更新后的速度和位置对载体在当前时刻k的姿态进行更新，得到更新后的姿态转换矩阵。
得到初始姿态转换矩阵和更新后的速度和位置之后，可以根据每一帧测得的陀螺仪输出进行姿态转换矩阵更新，更新后的姿态转换矩阵微分方程为：
Figure GDA0004255606470000087
其中，
Figure GDA0004255606470000088
为载体坐标系b相对于导航坐标系n在当前时刻k的姿态转换矩阵，/>
Figure GDA0004255606470000089
为姿态转换矩阵/>
Figure GDA00042556064700000810
的微分，/>
Figure GDA00042556064700000811
为当前时刻k载体坐标系b相对于导航坐标系n输出的角速度在载体坐标系b上的投影矢量，/>
Figure GDA00042556064700000812
×为投影矢量/>
Figure GDA00042556064700000813
的反对称矩阵。
由于陀螺仪输出的是载体坐标系b相对于惯性坐标系i的角速度
Figure GDA00042556064700000814
而角速度
Figure GDA00042556064700000815
不能直接测量获得，需对式(8)的微分方程作如下变换：
Figure GDA0004255606470000091
其中，
Figure GDA0004255606470000092
为当前时刻k载体坐标系b相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量，/>
Figure GDA0004255606470000093
为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量，/>
Figure GDA0004255606470000094
为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在导航坐标系n上的投影矢量，/>
Figure GDA0004255606470000095
×为投影矢量/>
Figure GDA0004255606470000096
的反对称矩阵，/>
Figure GDA0004255606470000097
为导航坐标系n相对于载体坐标系b在当前时刻k的姿态转换矩阵，由其定义可知，/>
Figure GDA0004255606470000098
是/>
Figure GDA0004255606470000099
的转置矩阵，即/>
Figure GDA00042556064700000910
投影矢量
Figure GDA00042556064700000911
即导航坐标系n相对于惯性坐标系i的旋转，其包含两个部分：地球自转引起的导航坐标系n旋转/>
Figure GDA00042556064700000912
以及惯性导航***在地球表面附近移动因地球表面弯曲引起的导航坐标系n旋转/>
Figure GDA00042556064700000913
则有/>
Figure GDA00042556064700000914
Figure GDA00042556064700000915
Figure GDA00042556064700000916
其中，
Figure GDA00042556064700000917
为当前时刻k地球自转角速度ωie在导航坐标系n上的投影矢量，/>
Figure GDA00042556064700000918
为当前时刻k导航坐标系n相对于地理系e输出的角速度在导航坐标系n上的投影矢量；VE(k-1)、VN(k-1)和VU(k-1)分别为载体在上一时刻k-1的东向速度、北向速度和天向速度，由上一时刻k-1的加速度计输出进行更新；RM、RN为地球曲率半径。
通过求解式(9)的微分方程即可求得当前时刻k的姿态转换矩阵
Figure GDA00042556064700000919
本实施例是基于东北天导航坐标系进行导航姿态解算，其姿态转换矩阵的定义式如下：
Figure GDA00042556064700000920
根据姿态转换矩阵的定义，再结合微分方程(9)的求解结果可以获得姿态转换矩阵中每个元素的具体值。
步骤4：判断步骤3中更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值，如果是，则按照如下公式(12)和(13)解算载体在当前时刻k的俯仰角、滚动角和方位角，具体公式为：
Figure GDA0004255606470000101
Figure GDA0004255606470000102
其中，θk为当前时刻k的俯仰角，γk为当前时刻k的滚动角，
Figure GDA0004255606470000103
为当前时刻k的方位角ψk的微分，/>
Figure GDA0004255606470000104
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000105
中第3行第2列的元素，/>
Figure GDA0004255606470000106
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000107
中第1行第3列的元素，/>
Figure GDA0004255606470000108
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000109
中第1行第1列的元素，/>
Figure GDA00042556064700001010
为Y轴陀螺在当前时刻k的角增量输出，/>
Figure GDA00042556064700001011
为Z轴陀螺在当前时刻k的角增量输出。
否则，按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角，具体公式为：
Figure GDA00042556064700001012
其中，
Figure GDA00042556064700001013
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700001014
中第3行第1列的元素，/>
Figure GDA00042556064700001015
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700001016
中第3行第3列的元素，/>
Figure GDA00042556064700001017
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700001018
中第1行第2列的元素，/>
Figure GDA00042556064700001019
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700001020
中第2行第2列的元素。
即当解算出的俯仰角未进入到±90°的阈值范围内时，按照式(14)进行俯仰角、滚动角和方位角的解算，当解算出的俯仰角接近到±90°的阈值范围时，按照式(12)和(13)进行俯仰角、滚动角和方位角的解算。
根据工程经验，本实施例中，设定阈值为0.9999，即
Figure GDA00042556064700001021
时，按照式(12)和(13)进行俯仰角、滚动角和方位角的解算，解出式(13)的微分方程即可解算出方位角。
在姿态解算时，需要注意解算角度的值域，θk的值域为[-90°，90°)，γk的值域为[-180°，180°)，ψk的值域为[0°，360°)。当载体绕滚动轴或方位轴有运动时，这种姿态解算就无法实时跟踪载体的方位角。本实施例中，初始姿态下，载体X轴转动对应俯仰角变化，载体Y轴转动对应滚动角变化，载体Z轴转动对应方位角变化，设顺时针为正，则在俯仰角绝对值大于89.2°时，方位角变化受到Y轴和Z轴两个轴向的角度变化影响，具体计算如式(13)。
值得注意的是，在俯仰角由89.5°增加到90°后继续增加0.5°时，由于俯仰角的值域限制，此时俯仰角的输出仍然为89.5°，但此时方位角变化了180°。因此，在俯仰角绝对值大于89.2°时的方位角解算中，增加一个判断公式(15)，即当
Figure GDA0004255606470000111
且俯仰角的绝对值大于89.2°时，当前时刻k的方位角的计算公式为：
Figure GDA0004255606470000112
其中，ψk为式(13)解算出的当前时刻k的方位角，ψ'k为式(15)解算出的当前时刻k的方位角，az(k)为Z轴加速计在当前时刻k的输出，az(k-1)为Z轴加速计在上一时刻k-1的输出。
步骤5：重复步骤2～4，解算载体在下一时刻k+1的俯仰角、滚动角和方位角，直到惯性导航结束或最后一帧加速度计和陀螺仪输出数据。
本实施例所提供的一种惯性导航***的全姿态导航解算方法，通过初始对准得到搭载惯性导航***的运动体(简称载体)初始姿态转换矩阵后，再通过三轴加速度计输出和陀螺仪输出进行每个时刻的姿态转换矩阵更新，可针对性地解决全姿态导航解算问题，实用性强，解决了在俯仰角绝对值大于89.2°时方位角姿态解算不准确的问题，避免了姿态监控软件发生误报的问题。
本实施例还提供一种惯性导航***的全姿态导航解算装置，包括：
初始对准单元，用于对搭载惯性导航***的载体速度、位置和姿态进行初始对准，计算初始姿态转换矩阵，如式(4)所示。
输入三轴加速度计增量数据和陀螺增量数据，数据格式要求以6列形式保存的纯数据文本，第一至三列分别为XYZ轴向的加速度增量数据，第四至六列分别为XYZ轴向的陀螺增量数据；输入当地重力加速度g，输入当地经度λ(单位为°)，输入当地纬度L(单位为°)；输入导航开始时间，一般默认为300s，前300s必须为静态数据，用于初始对准；导航解算结果从第301s开始输出。如图2所示，点击“导入导航数据”，即可输入导航解算所需的三轴加速度计增量数据和陀螺增量数据。
速度位置更新单元，用于根据所述初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新，如式(5)～(7)所示。
姿态更新单元，用于根据所述初始姿态转换矩阵、所述更新后的速度和位置对载体在当前时刻k的姿态进行更新，得到更新后的姿态转换矩阵，如式(9)所示。
解算单元，用于判断更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值，如果是，则按照如下公式解算载体在当前时刻k的俯仰角、滚动角和方位角，具体公式为：
Figure GDA0004255606470000121
Figure GDA0004255606470000122
Figure GDA0004255606470000123
其中，θk为当前时刻k的俯仰角，γk为当前时刻k的滚动角，
Figure GDA0004255606470000124
为当前时刻k的方位角ψk的微分，/>
Figure GDA0004255606470000125
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000126
中第3行第2列的元素，/>
Figure GDA0004255606470000127
为当前时刻k的姿态转换矩阵/>
Figure GDA0004255606470000128
中第1行第3列的元素，/>
Figure GDA0004255606470000129
为当前时刻k的姿态转换矩阵/>
Figure GDA00042556064700001210
中第1行第1列的元素，/>
Figure GDA00042556064700001211
为Y轴陀螺在当前时刻k的角增量输出，/>
Figure GDA00042556064700001212
为Z轴陀螺在当前时刻k的角增量输出；
否则，按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角，如式(14)所示。
如图2所示，点击“导航计算”，等待弹出“导航计算完毕”窗口，可在导航数据文件夹中找到命名为result.txt的文件，即为导航解算结果，导航解算结果文件为九列数据组成，第一至三列分别为导航解算俯仰角，滚动角和方位角；第四至六列分别为导航解算的东向速度误差，北向速度误差和天向速度误差；第七至九列分别为导航解算的东向位移误差，北向位移误差和天向位移误差。
以上所揭露的仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或变型，都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种惯性导航***的全姿态导航解算方法，其特征在于，包括以下步骤：
步骤1：对搭载惯性导航***的载体速度、位置和姿态进行初始对准，计算初始姿态转换矩阵；
步骤2：根据所述步骤1中的初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新；
步骤3：根据所述步骤1中的初始姿态转换矩阵、所述步骤2中更新后的速度和位置对载体在当前时刻k的姿态进行更新，得到更新后的姿态转换矩阵；
步骤4：判断所述步骤3中更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值，如果是，则按照如下公式解算载体在当前时刻k的俯仰角、滚动角和方位角，具体公式为：
Figure FDA0004255606460000011
Figure FDA0004255606460000012
Figure FDA0004255606460000013
其中，θk为当前时刻k的俯仰角，γk为当前时刻k的滚动角，
Figure FDA0004255606460000014
为当前时刻k的方位角ψk的微分，/>
Figure FDA0004255606460000015
为当前时刻k的姿态转换矩阵/>
Figure FDA0004255606460000016
中第3行第2列的元素，/>
Figure FDA0004255606460000017
为当前时刻k的姿态转换矩阵/>
Figure FDA0004255606460000018
中第1行第3列的元素，/>
Figure FDA0004255606460000019
为当前时刻k的姿态转换矩阵/>
Figure FDA00042556064600000110
中第1行第1列的元素，/>
Figure FDA00042556064600000111
为Y轴陀螺在当前时刻k的角增量输出，/>
Figure FDA00042556064600000112
为Z轴陀螺在当前时刻k的角增量输出；
否则，按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角；
步骤5：重复步骤2～4，解算载体在下一时刻k+1的俯仰角、滚动角和方位角，直到惯性导航结束。
2.如权利要求1所述的全姿态导航解算方法，其特征在于，所述步骤1中，初始姿态转换矩阵的计算过程为：
计算当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影，具体为：
gn＝[00g]T
Figure FDA00042556064600000113
其中，gn为当地重力加速度g在导航坐标系n上的投影，L为载体当前所在位置的纬度，
Figure FDA0004255606460000021
为地球自转角速度ωie在导航坐标系n上的投影；
计算当地重力加速度g和地球自转角速度ωie在载体坐标系b上的投影；
根据当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系计算初始姿态转换矩阵，所述初始姿态转换矩阵的表达式为：
Figure FDA0004255606460000022
其中，
Figure FDA0004255606460000023
为初始姿态转换矩阵，gb为当地重力加速度g在载体坐标系b上的投影，/>
Figure FDA0004255606460000024
为地球自转角速度ωie在载体坐标系b上的投影。
3.如权利要求1所述的全姿态导航解算方法，其特征在于，所述步骤2中，更新后载体的速度微分方程为：
Figure FDA0004255606460000025
Figure FDA0004255606460000026
Figure FDA0004255606460000027
Figure FDA0004255606460000028
其中，R为地球半径，
Figure FDA0004255606460000029
和/>
Figure FDA00042556064600000210
分别为东向速度、北向速度和天向速度的微分，VE(k)、VN(k)和VU(k)分别为载体在当前时刻k的东向速度、北向速度和天向速度，L(k-1)和h(k-1)分别为载体在上一时刻k-1的纬度和高度，fE(k)、fN(k)和fU(k)分别为载体在当前时刻k的东向比力、北向比力和天向比力，ωie为地球自转角速度，g为当地重力加速度；ax(k)、ay(k)、az(k)分别为当前时刻k时三轴加速度计X向、Y向、Z向的输出；/>
Figure FDA00042556064600000211
为当前时刻的姿态转换矩阵；
更新后载体的位置微分方程为：
Figure FDA0004255606460000031
Figure FDA0004255606460000032
Figure FDA0004255606460000033
其中，
Figure FDA0004255606460000034
为载体在当前时刻k的经度λ(k)的微分，/>
Figure FDA0004255606460000035
和/>
Figure FDA0004255606460000036
分别为载体在当前时刻k的纬度L(k)和高度h(k)的微分。
4.如权利要求1所述的全姿态导航解算方法，其特征在于，所述步骤3中，更新后的姿态转换矩阵微分方程为：
Figure FDA0004255606460000037
Figure FDA0004255606460000038
Figure FDA0004255606460000039
Figure FDA00042556064600000310
其中，
Figure FDA00042556064600000311
为载体坐标系b相对于导航坐标系n在当前时刻k的姿态转换矩阵，/>
Figure FDA00042556064600000312
为姿态转换矩阵/>
Figure FDA00042556064600000313
的微分，/>
Figure FDA00042556064600000314
为当前时刻k载体坐标系b相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量，/>
Figure FDA00042556064600000315
为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在导航坐标系n上的投影矢量，/>
Figure FDA00042556064600000316
为投影矢量/>
Figure FDA00042556064600000317
的反对称矩阵，/>
Figure FDA00042556064600000318
为当前时刻k地球自转角速度ωie在导航坐标系n上的投影，/>
Figure FDA00042556064600000319
为当前时刻k导航坐标系n相对于地理系e输出的角速度在导航坐标系n上的投影矢量，L(k-1)和h(k-1)分别为载体在上一时刻k-1的纬度和高度，VE(k-1)、VN(k-1)和VU(k-1)分别为载体在上一时刻k-1的东向速度、北向速度和天向速度，RM、RN为地球曲率半径。
5.如权利要求1～4中任一项所述的全姿态导航解算方法，其特征在于，所述步骤4中，当当前时刻k的姿态转换矩阵中第3行第2列元素的绝对值大于设定阈值，且俯仰角的绝对值大于89.2°时，当前时刻k的方位角的计算公式为：
Figure FDA0004255606460000041
其中，ψk为步骤4解算出的当前时刻k的方位角，ψ'k为俯仰角的绝对值大于89.2°时当前时刻k的方位角，az(k)为Z轴加速计在当前时刻k的输出，az(k-1)为Z轴加速计在上一时刻k-1的输出。
6.如权利要求1～4中任一项所述的全姿态导航解算方法，其特征在于，所述步骤4中，按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角的公式分别为：
Figure FDA0004255606460000042
Figure FDA0004255606460000043
Figure FDA0004255606460000044
其中，
Figure FDA0004255606460000045
为当前时刻k的姿态转换矩阵/>
Figure FDA0004255606460000046
中第3行第1列的元素，/>
Figure FDA0004255606460000047
为当前时刻k的姿态转换矩阵/>
Figure FDA0004255606460000048
中第3行第3列的元素，/>
Figure FDA0004255606460000049
为当前时刻k的姿态转换矩阵/>
Figure FDA00042556064600000410
中第1行第2列的元素，/>
Figure FDA00042556064600000411
为当前时刻k的姿态转换矩阵/>
Figure FDA00042556064600000412
中第2行第2列的元素。
7.一种惯性导航***的全姿态导航解算装置，其特征在于，包括：
初始对准单元，用于对搭载惯性导航***的载体速度、位置和姿态进行初始对准，计算初始姿态转换矩阵；
速度位置更新单元，用于根据所述初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新；
姿态更新单元，用于根据所述初始姿态转换矩阵、所述更新后的速度和位置对载体在当前时刻k的姿态进行更新，得到更新后的姿态转换矩阵；
解算单元，用于判断更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值，如果是，则按照如下公式解算载体在当前时刻k的俯仰角、滚动角和方位角，具体公式为：
Figure FDA00042556064600000413
Figure FDA00042556064600000414
Figure FDA00042556064600000415
其中，θk为当前时刻k的俯仰角，γk为当前时刻k的滚动角，
Figure FDA0004255606460000051
为当前时刻k的方位角ψk的微分，/>
Figure FDA0004255606460000052
为当前时刻k的姿态转换矩阵/>
Figure FDA0004255606460000053
中第3行第2列的元素，/>
Figure FDA0004255606460000054
为当前时刻k的姿态转换矩阵/>
Figure FDA0004255606460000055
中第1行第3列的元素，/>
Figure FDA0004255606460000056
为当前时刻k的姿态转换矩阵/>
Figure FDA0004255606460000057
中第1行第1列的元素，/>
Figure FDA0004255606460000058
为Y轴陀螺在当前时刻k的角增量输出，/>
Figure FDA0004255606460000059
为Z轴陀螺在当前时刻k的角增量输出；
否则，按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角。
8.一种惯性导航***，其特征在于：包括如权利要求7所述的全姿态导航解算装置。
CN202110808144.XA 2021-07-16 2021-07-16 一种惯性导航***及其全姿态导航解算方法与装置 Active CN113587925B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110808144.XA CN113587925B (zh) 2021-07-16 2021-07-16 一种惯性导航***及其全姿态导航解算方法与装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110808144.XA CN113587925B (zh) 2021-07-16 2021-07-16 一种惯性导航***及其全姿态导航解算方法与装置

Publications (2)

Publication Number Publication Date
CN113587925A CN113587925A (zh) 2021-11-02
CN113587925B true CN113587925B (zh) 2023-07-07

Family

ID=78247834

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110808144.XA Active CN113587925B (zh) 2021-07-16 2021-07-16 一种惯性导航***及其全姿态导航解算方法与装置

Country Status (1)

Country Link
CN (1) CN113587925B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114485641B (zh) * 2022-01-24 2024-03-26 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN115560756B (zh) * 2022-08-26 2023-07-04 北京开拓航宇导控科技有限公司 一种发射坐标系下微型自寻的导弹捷联导航方法
CN116048146B (zh) * 2023-03-31 2023-06-13 中国船舶集团有限公司第七〇七研究所 面向旋转光纤陀螺惯导的角速度平滑控制方法
CN117091596B (zh) * 2023-10-09 2024-06-21 腾讯科技（深圳）有限公司 一种姿态信息获取方法以及相关设备

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104677358A (zh) * 2013-11-29 2015-06-03 哈尔滨恒誉名翔科技有限公司 一种微捷联航姿***全姿态控制器
CN105203098B (zh) * 2015-10-13 2018-10-02 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法
CN106153073B (zh) * 2016-06-21 2018-10-12 东南大学 一种全姿态捷联惯导***的非线性初始对准方法
JP6922641B2 (ja) * 2017-10-13 2021-08-18 株式会社Ｊｖｃケンウッド 角速度導出装置および角速度導出方法
CN109115212B (zh) * 2018-10-30 2022-04-12 中国船舶重工集团公司第七0七研究所 一种惯导***全范围姿态角提取方法
CN110132269A (zh) * 2019-06-10 2019-08-16 西北工业大学 一种导弹高精度垂直发射初始姿态获取方法
CN111780749B (zh) * 2020-05-26 2022-06-03 北京航天控制仪器研究所 一种变轨机动飞机全姿态惯性导航的姿态控制方法
CN111721291B (zh) * 2020-07-17 2021-12-07 河北斐然科技有限公司 一种发射系下捷联惯组导航的工程算法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Also Published As

Publication number Publication date
CN113587925A (zh) 2021-11-02

Similar Documents

Publication Publication Date Title
CN113587925B (zh) 一种惯性导航***及其全姿态导航解算方法与装置
KR102454882B1 (ko) 차량 주행위치 추산 방법, 장치, 기기, 저장매체 및 프로그램
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航***及方法
JP7036080B2 (ja) 慣性航法装置
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN114485641A (zh) 一种基于惯导卫导方位融合的姿态解算方法及装置
CN112880669A (zh) 一种航天器星光折射和单轴旋转调制惯性组合导航方法
CN108871323B (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN115855048A (zh) 一种多传感器融合位姿估计方法
CN115655271B (zh) 一种动态条件下的大范围姿态角提取方法
CN111141283A (zh) 一种通过地磁数据判断行进方向的方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN109945857B (zh) 一种面向不动产实地测量的车载惯性定位方法及其装置
CN113447024B (zh) 基于扩展克雷洛夫角的惯性导航姿态角解算方法和***
CN113447025B (zh) 基于克雷洛夫角的惯性导航高精度姿态角解算方法和***
CN115855063A (zh) 基于绝对姿态递推修正的交会对接敏感器数据预处理方法
CN111649738B (zh) 微重力场下的加速度计初始姿态解算方法
CN114526729A (zh) 一种基于冗余技术的mems惯性定位***航向优化方法
WO2021012635A1 (zh) 一种基于陀螺仪信息的惯性导航方法
CN113670297A (zh) 一种基于mems和轮式里程计的离线定位方法
CN113051757B (zh) 一种捷联惯导广义psi角误差模型构建方法
CN118225086B (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