一种惯性导航系统及其全姿态导航解算方法与装置
阅读说明：本技术 一种惯性导航系统及其全姿态导航解算方法与装置 (Inertial navigation system and full-attitude navigation resolving method and device thereof ) 是由 王胜兰 刘晓庆 于 2021-07-16 设计创作，主要内容包括：本发明公开了一种惯性导航系统及其全姿态导航解算方法与装置,包括初始对准,计算初始姿态转换矩阵；对载体的速度、位置和姿态进行更新,得到更新后的姿态转换矩阵；在更新后的姿态转换矩阵中第3行第2列元素的绝对值大于设定阈值时采用全姿态导航解算俯仰角、滚动角和方位角,在更新后的姿态转换矩阵中第3行第2列元素的绝对值小于等于设定阈值时采用常规方法导航解算俯仰角、滚动角和方位角；通过三轴加速度计增量和陀螺增量进行全姿态下载体姿态角、运动速度、运动位移的计算,且能够对俯仰角绝对值大于89.2°时的方位角进行跟踪,解决了在俯仰角绝对值大于89.2°时的方位角姿态解算不准确的问题。(The invention discloses an inertial navigation system and a full-attitude navigation resolving method and a full-attitude navigation resolving device thereof, which comprise the steps of initial alignment and calculation of an initial attitude transformation matrix; updating the speed, position and posture of the carrier to obtain an updated posture conversion matrix; when the absolute value of the row 3 and column 2 elements in the updated attitude conversion matrix is greater than a set threshold value, full-attitude navigation is adopted to solve a pitch angle, a roll angle and an azimuth angle, and when the absolute value of the row 3 and column 2 elements in the updated attitude conversion matrix is less than or equal to the set threshold value, a conventional method is adopted to solve the pitch angle, the roll angle and the azimuth angle in a navigation manner; the attitude angle, the movement speed and the movement displacement of the carrier under the full attitude are calculated through the triaxial accelerometer increment and the gyroscope increment, the azimuth angle when the absolute value of the pitch angle is more than 89.2 degrees can be tracked, and the problem of inaccurate attitude calculation of the azimuth angle when the absolute value of the pitch angle is more than 89.2 degrees is solved.)
技术领域
本发明属于惯性导航技术领域，尤其涉及一种惯性导航系统及其全姿态导航解算方法与装置。
背景技术
在惯性导航姿态解算过程中，当俯仰角接近±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
其中，gn为当地重力加速度g在导航坐标系n上的投影，L为载体当前所在位置的纬度，为地球自转角速度ωie在导航坐标系n上的投影；
计算当地重力加速度g和地球自转角速度ωie在载体坐标系b上的投影；
根据当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系计算初始姿态转换矩阵，所述初始姿态转换矩阵的表达式为：
其中，为初始姿态转换矩阵，gb为当地重力加速度g在载体坐标系b上的投影，为地球自转角速度ωie在载体坐标系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向的输出；为当前时刻的姿态转换矩阵；
更新后载体的位置微分方程为：
其中，为载体在当前时刻k的经度λ(k)的微分，和分别为载体在当前时刻k的纬度L(k)和高度h(k)的微分。
进一步地，所述步骤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之间的坐标转换关系为：
其中，αn、αb分别为空间向量α在导航坐标系n、载体坐标系b的坐标表示，为载体坐标系b到导航坐标系n的姿态转换矩阵。
本实施例中，利用当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系来计算初始姿态转换矩阵，具体计算过程为：
1.1计算当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影，具体为：
gn＝[0 0 g]T (2)
其中，gn为当地重力加速度g在导航坐标系n上的投影，L为载体当前所在位置的纬度，为地球自转角速度ωie在导航坐标系n上的投影。
1.2计算当地重力加速度g和地球自转角速度ωie在载体坐标系b上的投影gb和其中，gb为300s静态数据中三轴加速度计输出的均值，为300s静态数据中陀螺仪输出的均值。
步骤1.1与步骤1.2无先后顺序，可以同时进行。
1.3根据当地重力加速度g和地球自转角速度ωie在导航坐标系n上的投影与在载体坐标系b上的投影之间的坐标转换关系(即式(1))计算初始姿态转换矩阵，初始姿态转换矩阵的表达式为：
其中，为初始姿态转换矩阵，gb为当地重力加速度g在载体坐标系b上的投影，为地球自转角速度ωie在载体坐标系b上的投影。
步骤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的东向比力、北向比力和天向比力由三轴加速度计的输出左乘姿态转换矩阵求得，具体为：
其中，ax(k)、ay(k)、az(k)分别为当前时刻k时三轴加速度计X向、Y向、Z向的输出；为当前时刻的姿态转换矩阵。
解算速度微分方程式(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上的投影矢量，×为投影矢量的反对称矩阵。
由于陀螺仪输出的是载体坐标系b相对于惯性坐标系i的角速度而角速度不能直接测量获得，需对式(8)的微分方程作如下变换：
其中，为当前时刻k载体坐标系b相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量，为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在载体坐标系b上的投影矢量，为当前时刻k导航坐标系n相对于惯性坐标系i输出的角速度在导航坐标系n上的投影矢量，×为投影矢量的反对称矩阵，为导航坐标系n相对于载体坐标系b在当前时刻k的姿态转换矩阵，由其定义可知，是的转置矩阵，即
投影矢量即导航坐标系n相对于惯性坐标系i的旋转，其包含两个部分：地球自转引起的导航坐标系n旋转以及惯性导航系统在地球表面附近移动因地球表面弯曲引起的导航坐标系n旋转则有
其中，为当前时刻k地球自转角速度ωie在导航坐标系n上的投影矢量，为当前时刻k导航坐标系n相对于地理系e输出的角速度在导航坐标系n上的投影矢量；VE(k-1)、VN(k-1)和VU(k-1)分别为载体在上一时刻k-1的东向速度、北向速度和天向速度，由上一时刻k-1的加速度计输出进行更新；RM、RN为地球曲率半径。
通过求解式(9)的微分方程即可求得当前时刻k的姿态转换矩阵
本实施例是基于东北天导航坐标系进行导航姿态解算，其姿态转换矩阵的定义式如下：
根据姿态转换矩阵的定义，再结合微分方程(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)进行俯仰角、滚动角和方位角的解算。
根据工程经验，本实施例中，设定阈值为0.9999，即时，按照式(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)，即当且俯仰角的绝对值大于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的文件，即为导航解算结果，导航解算结果文件为九列数据组成，第一至三列分别为导航解算俯仰角，滚动角和方位角；第四至六列分别为导航解算的东向速度误差，北向速度误差和天向速度误差；第七至九列分别为导航解算的东向位移误差，北向位移误差和天向位移误差。
以上所揭露的仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或变型，都应涵盖在本发明的保护范围之内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种航天器空间自主交会对接相对导航方法