CN113587925B - 一种惯性导航***及其全姿态导航解算方法与装置 - Google Patents
一种惯性导航***及其全姿态导航解算方法与装置 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 19
- 239000011159 matrix material Substances 0.000 claims abstract description 104
- 238000006243 chemical reaction Methods 0.000 claims abstract description 102
- 238000004364 calculation method Methods 0.000 claims abstract description 20
- 238000005096 rolling process Methods 0.000 claims abstract description 11
- 238000007796 conventional method Methods 0.000 claims abstract description 9
- 230000001133 acceleration Effects 0.000 claims description 23
- 230000005484 gravity Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 5
- 230000004069 differentiation Effects 0.000 claims description 2
- 230000017525 heat dissipation Effects 0.000 claims 1
- 238000006073 displacement reaction Methods 0.000 abstract description 6
- 230000008859 change Effects 0.000 description 5
- 230000003068 static effect Effects 0.000 description 5
- 238000013459 approach Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005530 etching Methods 0.000 description 1
- 230000008676 import Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
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的俯仰角、滚动角和方位角,具体公式为:
其中,θk为当前时刻k的俯仰角,γk为当前时刻k的滚动角,为当前时刻k的方位角ψk的微分,/>为当前时刻k的姿态转换矩阵/>中第3行第2列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第3列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第1列的元素,/>为Y轴陀螺在当前时刻k的角增量输出,/>为Z轴陀螺在当前时刻k的角增量输出;
否则,按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角;
步骤5:重复步骤2~4,解算载体在下一时刻k+1的俯仰角、滚动角和方位角,直到惯性导航结束。
进一步地,所述步骤1中,初始姿态转换矩阵的计算过程为:
计算当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影,具体为:
gn=[0 0 g]T
计算当地重力加速度g和地球自转角速度ωie在载体坐标系b上的投影;
根据当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系计算初始姿态转换矩阵,所述初始姿态转换矩阵的表达式为:
进一步地,所述步骤2中,更新后载体的速度微分方程为:
其中,R为地球半径,和/>分别为东向速度、北向速度和天向速度的微分,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向的输出;/>为当前时刻的姿态转换矩阵;
更新后载体的位置微分方程为:
进一步地,所述步骤3中,更新后的姿态转换矩阵微分方程为:
其中,为载体坐标系b相对于导航坐标系n在当前时刻k的姿态转换矩阵,/>为姿态转换矩阵/>的微分,/>为当前时刻k载体坐标系b相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量,/>为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在导航坐标系n上的投影矢量,/>为投影矢量/>的反对称矩阵,/>为当前时刻k地球自转角速度ωie在导航坐标系n上的投影,/>为当前时刻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的方位角的计算公式为:
其中,ψk为步骤4解算出的当前时刻k的方位角,ψ'k为俯仰角的绝对值大于89.2°时当前时刻k的方位角,az(k)为Z轴加速计在当前时刻k的输出,az(k-1)为Z轴加速计在上一时刻k-1的输出。
进一步地,所述步骤4中,按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角的公式分别为:
其中,为当前时刻k的姿态转换矩阵/>中第3行第1列的元素,/>为当前时刻k的姿态转换矩阵/>中第3行第3列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第2列的元素,/>为当前时刻k的姿态转换矩阵/>中第2行第2列的元素。
本发明还提供一种惯性导航***的全姿态导航解算装置,包括:
初始对准单元,用于对搭载惯性导航***的载体速度、位置和姿态进行初始对准,计算初始姿态转换矩阵;
速度位置更新单元,用于根据所述初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新;
姿态更新单元,用于根据所述初始姿态转换矩阵、所述更新后的速度和位置对载体在当前时刻k的姿态进行更新,得到更新后的姿态转换矩阵;
解算单元,用于判断更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值,如果是,则按照如下公式解算载体在当前时刻k的俯仰角、滚动角和方位角,具体公式为:
其中,θk为当前时刻k的俯仰角,γk为当前时刻k的滚动角,为当前时刻k的方位角ψk的微分,/>为当前时刻k的姿态转换矩阵/>中第3行第2列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第3列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第1列的元素,/>为Y轴陀螺在当前时刻k的角增量输出,/>为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之间的坐标转换关系为:
本实施例中,利用当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系来计算初始姿态转换矩阵,具体计算过程为:
1.1计算当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影,具体为:
gn=[0 0 g]T (2)
步骤1.1与步骤1.2无先后顺序,可以同时进行。
1.3根据当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系(即式(1))计算初始姿态转换矩阵,初始姿态转换矩阵的表达式为:
步骤2:根据步骤1中的初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新。
得到初始姿态转换矩阵后,可以更新任意时刻的速度和位置。本实施例中,导航坐标系为东北天坐标系,更新后载体在当前时刻k的速度微分方程为:
其中,R为地球半径,和/>分别为东向速度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的东向比力、北向比力和天向比力由三轴加速度计的输出左乘姿态转换矩阵求得,具体为:
解算速度微分方程式(5)即可求得载体在当前时刻k的东向速度VE(k)、北向速度VN(k)和天向速度VU(k)。当前时刻k的位置更新是基于速度更新的,具体为:
其中,为载体在当前时刻k的经度λ(k)的微分,/>和/>分别为载体在当前时刻k的纬度L(k)和高度h(k)的微分。解算位置微分方程式(7)即可求得载体在当前时刻k的经度λ(k)、纬度L(k)和高度h(k)。
步骤3:根据步骤1中的初始姿态转换矩阵、步骤2中更新后的速度和位置对载体在当前时刻k的姿态进行更新,得到更新后的姿态转换矩阵。
得到初始姿态转换矩阵和更新后的速度和位置之后,可以根据每一帧测得的陀螺仪输出进行姿态转换矩阵更新,更新后的姿态转换矩阵微分方程为:
其中,为载体坐标系b相对于导航坐标系n在当前时刻k的姿态转换矩阵,/>为姿态转换矩阵/>的微分,/>为当前时刻k载体坐标系b相对于导航坐标系n输出的角速度在载体坐标系b上的投影矢量,/>×为投影矢量/>的反对称矩阵。
其中,为当前时刻k载体坐标系b相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量,/>为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量,/>为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在导航坐标系n上的投影矢量,/>×为投影矢量/>的反对称矩阵,/>为导航坐标系n相对于载体坐标系b在当前时刻k的姿态转换矩阵,由其定义可知,/>是/>的转置矩阵,即/>
其中,为当前时刻k地球自转角速度ωie在导航坐标系n上的投影矢量,/>为当前时刻k导航坐标系n相对于地理系e输出的角速度在导航坐标系n上的投影矢量;VE(k-1)、VN(k-1)和VU(k-1)分别为载体在上一时刻k-1的东向速度、北向速度和天向速度,由上一时刻k-1的加速度计输出进行更新;RM、RN为地球曲率半径。
本实施例是基于东北天导航坐标系进行导航姿态解算,其姿态转换矩阵的定义式如下:
根据姿态转换矩阵的定义,再结合微分方程(9)的求解结果可以获得姿态转换矩阵中每个元素的具体值。
步骤4:判断步骤3中更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值,如果是,则按照如下公式(12)和(13)解算载体在当前时刻k的俯仰角、滚动角和方位角,具体公式为:
其中,θk为当前时刻k的俯仰角,γk为当前时刻k的滚动角,为当前时刻k的方位角ψk的微分,/>为当前时刻k的姿态转换矩阵/>中第3行第2列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第3列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第1列的元素,/>为Y轴陀螺在当前时刻k的角增量输出,/>为Z轴陀螺在当前时刻k的角增量输出。
否则,按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角,具体公式为:
其中,为当前时刻k的姿态转换矩阵/>中第3行第1列的元素,/>为当前时刻k的姿态转换矩阵/>中第3行第3列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第2列的元素,/>为当前时刻k的姿态转换矩阵/>中第2行第2列的元素。
即当解算出的俯仰角未进入到±90°的阈值范围内时,按照式(14)进行俯仰角、滚动角和方位角的解算,当解算出的俯仰角接近到±90°的阈值范围时,按照式(12)和(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),即当且俯仰角的绝对值大于89.2°时,当前时刻k的方位角的计算公式为:
其中,ψ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的俯仰角、滚动角和方位角,具体公式为:
其中,θk为当前时刻k的俯仰角,γk为当前时刻k的滚动角,为当前时刻k的方位角ψk的微分,/>为当前时刻k的姿态转换矩阵/>中第3行第2列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第3列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第1列的元素,/>为Y轴陀螺在当前时刻k的角增量输出,/>为Z轴陀螺在当前时刻k的角增量输出;
否则,按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角,如式(14)所示。
如图2所示,点击“导航计算”,等待弹出“导航计算完毕”窗口,可在导航数据文件夹中找到命名为result.txt的文件,即为导航解算结果,导航解算结果文件为九列数据组成,第一至三列分别为导航解算俯仰角,滚动角和方位角;第四至六列分别为导航解算的东向速度误差,北向速度误差和天向速度误差;第七至九列分别为导航解算的东向位移误差,北向位移误差和天向位移误差。
以上所揭露的仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到变化或变型,都应涵盖在本发明的保护范围之内。
Claims (8)
1.一种惯性导航***的全姿态导航解算方法,其特征在于,包括以下步骤:
步骤1:对搭载惯性导航***的载体速度、位置和姿态进行初始对准,计算初始姿态转换矩阵;
步骤2:根据所述步骤1中的初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新;
步骤3:根据所述步骤1中的初始姿态转换矩阵、所述步骤2中更新后的速度和位置对载体在当前时刻k的姿态进行更新,得到更新后的姿态转换矩阵;
步骤4:判断所述步骤3中更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值,如果是,则按照如下公式解算载体在当前时刻k的俯仰角、滚动角和方位角,具体公式为:
其中,θk为当前时刻k的俯仰角,γk为当前时刻k的滚动角,为当前时刻k的方位角ψk的微分,/>为当前时刻k的姿态转换矩阵/>中第3行第2列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第3列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第1列的元素,/>为Y轴陀螺在当前时刻k的角增量输出,/>为Z轴陀螺在当前时刻k的角增量输出;
否则,按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角;
步骤5:重复步骤2~4,解算载体在下一时刻k+1的俯仰角、滚动角和方位角,直到惯性导航结束。
2.如权利要求1所述的全姿态导航解算方法,其特征在于,所述步骤1中,初始姿态转换矩阵的计算过程为:
计算当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影,具体为:
gn=[00g]T
计算当地重力加速度g和地球自转角速度ωie在载体坐标系b上的投影;
根据当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系计算初始姿态转换矩阵,所述初始姿态转换矩阵的表达式为:
3.如权利要求1所述的全姿态导航解算方法,其特征在于,所述步骤2中,更新后载体的速度微分方程为:
其中,R为地球半径,和/>分别为东向速度、北向速度和天向速度的微分,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向的输出;/>为当前时刻的姿态转换矩阵;
更新后载体的位置微分方程为:
4.如权利要求1所述的全姿态导航解算方法,其特征在于,所述步骤3中,更新后的姿态转换矩阵微分方程为:
其中,为载体坐标系b相对于导航坐标系n在当前时刻k的姿态转换矩阵,/>为姿态转换矩阵/>的微分,/>为当前时刻k载体坐标系b相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量,/>为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在导航坐标系n上的投影矢量,/>为投影矢量/>的反对称矩阵,/>为当前时刻k地球自转角速度ωie在导航坐标系n上的投影,/>为当前时刻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为地球曲率半径。
7.一种惯性导航***的全姿态导航解算装置,其特征在于,包括:
初始对准单元,用于对搭载惯性导航***的载体速度、位置和姿态进行初始对准,计算初始姿态转换矩阵;
速度位置更新单元,用于根据所述初始姿态转换矩阵对载体在当前时刻k的速度和位置进行更新;
姿态更新单元,用于根据所述初始姿态转换矩阵、所述更新后的速度和位置对载体在当前时刻k的姿态进行更新,得到更新后的姿态转换矩阵;
解算单元,用于判断更新后的姿态转换矩阵中第3行第2列元素的绝对值是否大于设定阈值,如果是,则按照如下公式解算载体在当前时刻k的俯仰角、滚动角和方位角,具体公式为:
其中,θk为当前时刻k的俯仰角,γk为当前时刻k的滚动角,为当前时刻k的方位角ψk的微分,/>为当前时刻k的姿态转换矩阵/>中第3行第2列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第3列的元素,/>为当前时刻k的姿态转换矩阵/>中第1行第1列的元素,/>为Y轴陀螺在当前时刻k的角增量输出,/>为Z轴陀螺在当前时刻k的角增量输出;
否则,按照常规方法解算载体在当前时刻k的俯仰角、滚动角和方位角。
8.一种惯性导航***,其特征在于:包括如权利要求7所述的全姿态导航解算装置。
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)
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)
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 | 株式会社Jvcケンウッド | 角速度導出装置および角速度導出方法 |
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辅助的捷联惯导组合导航方法 |
-
2021
- 2021-07-16 CN CN202110808144.XA patent/CN113587925B/zh active Active
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 |