CN117516465B

CN117516465B - 一种避免跳变的惯导***姿态角提取方法

Info

Publication number: CN117516465B
Application number: CN202410010222.5A
Authority: CN
Inventors: 付博; 邱宝良; 刘建; 王浩; 郭小丽
Original assignee: Beijing Shendao Technology Co ltd
Current assignee: Beijing Shendao Technology Co ltd
Priority date: 2024-01-04
Filing date: 2024-01-04
Publication date: 2024-03-19
Anticipated expiration: 2044-01-04
Also published as: CN117516465A

Abstract

本发明提供一种避免跳变的惯导***姿态角提取方法，根据惯导***解算姿态矩阵的元素得到俯仰角,判断俯仰角绝对值是否小于K°，如果是，更新航向角；如果否，不更新航向角，默认采用上周期提取的航向角；判断俯仰角是否大于零，如果是，通过横滚角与航向角之差提取横滚角；如果否，通过横滚角与航向角之和提取横滚角；最后对横滚角和航向角的取值范围进行规范化。本发明通过计算横滚角与航向角的和或差来提取横滚角，在全姿态范围内均不存在奇异性；当俯仰角处于±90°附近时，通过不更新航向角有效避免了姿态角的不确定跳变，本方法不改变欧拉角定义，不损失姿态角提取精度，有效避免了横滚角和航向角的无序跳变，满足用户和控制***的使用习惯。

Description

一种避免跳变的惯导***姿态角提取方法

技术领域

本发明涉及测量测试技术领域，具体涉及一种避免跳变的惯导***姿态角提取方法。

背景技术

姿态角是载体坐标系与地理坐标系之间角位置关系最直观的描述方式，包含俯仰角、横滚角和航向角三个欧拉角。俯仰角表示载体纵轴与水平面之间的夹角，横滚角表示载体纵向对称面与纵轴所在铅垂平之间的夹角，航向角表示载体纵轴在水平面上的投影与当地子午线北向之间的夹角。欧拉角表述方式简单直观，容易理解，符合用户使用习惯，因此获得普遍应用并形成一系列经典的显示和控制方法。

从欧拉角定义来看，当俯仰角处于±90°时，飞行器的纵轴在水平面内的投影变成一个点，纵向铅垂面退化成纵向对称平面内的一条直线。所以从物理意义上看，尽管飞行器在空间中有确定的空间角位置, 但根本不存在有意义的航向角和横滚角, 这是姿态角定义上的奇异点, 反映了用姿态角描述刚体转动的局限性。

惯导***通常采用四元数或姿态矩阵微分方程进行姿态更新，导航算法本身没有奇异点，可以全姿态工作，姿态角提取仅仅是参数表达问题。在垂直条件下，航向角和横滚角都沿垂线方向定义，实际上相当于失去了一个自由度，航向角与横滚角的和或差可以确定，但无法提取唯一确定的航向角和横滚角信息。也就是说，虽然飞行器所处的空间角位置关系是唯一确定的，但可以有无数组欧拉角可以描述该位置，即欧拉角描述坐标转换关系存在多值性问题。

文献“惯性导航，科学出版社，2020，第三版，p260”给出了一种根据惯导***姿态矩阵提取姿态角的经典方法。方法根据姿态矩阵元素的反正弦和反正切函数求解俯仰角、横滚角和航向角，并通过真值表确定横滚角和航向角所在的象限。该方法能够在全范围内提取欧拉角信息，但当俯仰角处于±90°附近时，反正切函数内的数值出现分子分母均接近0的情况，横滚角和航向角的提取结果出现多值性，存在不确定跳变。横滚角和航向角的跳变本身是参数表达问题，不影响导航解算过程，但容易给用户或控制***造成不必要的困扰。

因此，需要一种可以解决全范围姿态角提取的角度跳变问题的方法。

发明内容

本发明是为了解决全范围姿态角提取的角度跳变问题，本发明提供一种避免跳变的惯导***姿态角提取方法，根据俯仰角计算结果判断提取流程，在俯仰角绝对值小于阈值时正常更新航向角，当俯仰角绝对值接近90度时默认采用上周期航向角，在已知航向角情况下，通过矩阵元素代数和计算横滚角与航向角的和或差来提取横滚角。该方法算法简洁，不存在原理性误差，可以有效解决垂直状态下航向角和横滚角的跳变问题。

本发明提供一种避免跳变的惯导***姿态角提取方法，包括以下步骤：

S1、根据惯导***解算姿态矩阵的元素得到俯仰角/>；

姿态矩阵为：

；

俯仰角为：/>；

其中，为姿态矩阵/>的第/>行第/>列元素，b为载体坐标系，n为导航坐标系；

S2、判断俯仰角绝对值是否小于K°，其中，K=90-k，k≤1，k为正数，如果是，更新航向角/>；如果否，不更新航向角/>，默认采用上周期提取的航向角/>；

S3、判断俯仰角是否大于零，如果是，进入步骤S4；如果否，进入步骤S5；

S4、通过横滚角与航向角之差提取横滚角/>，进入步骤S6；

S5、通过横滚角与航向角之和提取横滚角/>，进入步骤S6;

S6、对横滚角和航向角的取值范围进行规范化，得到规范化后横滚角和规范化后航向角/>；

规范化后航向角的值域为[0, />)：/>；

规范化后横滚角的值域为[/>，/>)：

；

一种避免跳变的惯导***姿态角提取方法完成。

本发明所述的一种避免跳变的惯导***姿态角提取方法，作为优选方式，步骤S1中，的值域为[/>，/>)。

本发明所述的一种避免跳变的惯导***姿态角提取方法，作为优选方式，步骤S2中，航向角为：

；

其中：atan2为标准库反正切函数，值域为[，/>)。

本发明所述的一种避免跳变的惯导***姿态角提取方法，作为优选方式，步骤S2中，K为89.9992°。

本发明所述的一种避免跳变的惯导***姿态角提取方法，作为优选方式，步骤S4中，横滚角为：

。

本发明所述的一种避免跳变的惯导***姿态角提取方法，作为优选方式，横滚角与航向角之差为：

。

本发明所述的一种避免跳变的惯导***姿态角提取方法，作为优选方式，对姿态矩阵的元素进行作差和求和处理得到：

。

本发明所述的一种避免跳变的惯导***姿态角提取方法，作为优选方式，步骤S5中，横滚角为：

。

本发明所述的一种避免跳变的惯导***姿态角提取方法，作为优选方式，横滚角与航向角之和为：

。

传统姿态提取方法中，由于载体的姿态角解算存在奇异点，惯导***在导航解算过程中，总是通过四元数或姿态矩阵方式进行姿态解算，然后再从姿态矩阵中提取姿态角供用户或控制***使用。

姿态矩阵的构成为：

；

传统方法直接根据姿态矩阵的元素计算俯仰角/>、横滚角/>和航向角/>：

；（1）

；（2）

；（3）

其中符号表示姿态矩阵/>的第/>行第/>列元素，atan2为标准库反正切函数，值域为[/>，/>）。

从数学公式上看，式（2）等价于：

；

区别仅在于atan2函数可以进一步确定值域的象限。可见，当俯仰角趋于±90°时，趋于0，在式（2）中横滚角/>计算式的分子/>趋于0，分母也趋于0。由于分子分母同时趋于0，分子分母的微小变化就会导致/>计算结果出现剧烈波动。同样，式（3）计算航向角/>也存在同样的情况。

从物理意义上看，在垂直条件下，航向角和横滚角都沿垂线方向定义，实际上相当于失去了一个自由度，航向角与横滚角的和或差可以确定，但无法提取唯一确定的航向角和横滚角信息。也就是说，虽然飞行器所处的空间角位置关系是唯一确定的，但可以有无数组欧拉角可以描述该位置，即欧拉角描述坐标转换关系存在多值性问题，因此正常提取过程会出现无序跳变。

本发明对姿态矩阵的元素进行加减并化简，得到：

；（4）

；（5）

当俯仰角为90°时，由式（4）可得：

；（6）

当俯仰角为-90°时，由式（5）可得：

；（7）

可见，当俯仰趋于±90°时，和/>仍然可以准确求解，这也与物理意义一致：在垂直条件下，航向角和横滚角都沿垂线方向定义，实际上相当于失去了一个自由度，航向角与横滚角的和或差可以确定，但无法提取唯一确定的航向角和横滚角信息。

根据式（4）可得：

；

根据式（5）可得：

；

因此，本发明使用式（1）计算俯仰角，不存在奇异性，但不再采用式（2）和（3）直接计算横滚角和航向角。

获得俯仰角后，当判断俯仰角不在±90°附近时，采用式（3）更新航向角没有奇异点，然后通过式（8）-（9）获得或/>后再求解横滚角/>。

当判断俯仰角在±90°附近时，式（3）计算航向角存在奇异，因此不再更新，相当于使用了前一周期解算的航向角，也就是在保证航向角不变的情况下计算横滚角，这样横滚角也不会发生跳变，从而实现了避免跳变的目的。

本发明具有以下优点：

本发明通过计算横滚角与航向角的和或差来提取横滚角，在全姿态范围内均不存在奇异性；当俯仰角处于±90°附近时，通过不更新航向角有效避免了姿态角的不确定跳变。该方法不改变欧拉角定义，不损失姿态角提取精度，且有效避免了横滚角和航向角的无序跳变，从而满足用户和控制***的使用习惯。

附图说明

图1为一种避免跳变的惯导***姿态角提取方法流程图；

图2a为现有技术通过导航解算姿态矩阵提取的俯仰角示意图；

图2b为现有技术通过导航解算姿态矩阵提取的横滚角示意图；

图2c为现有技术通过导航解算姿态矩阵提取的航向角示意图；

图3a为一种避免跳变的惯导***姿态角提取方法通过导航解算姿态矩阵提取的俯仰角示意图；

图3b为一种避免跳变的惯导***姿态角提取方法通过导航解算姿态矩阵提取的横滚角示意图；

图3c为一种避免跳变的惯导***姿态角提取方法通过导航解算姿态矩阵提取的航向角示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

实施例1

如图1所示，一种避免跳变的惯导***姿态角提取方法，包括以下步骤：

步骤1：参照图1，首先根据惯导***解算姿态矩阵的元素计算俯仰角/>。

姿态矩阵的构成为：

；

根据姿态矩阵的元素计算俯仰角/>：

；

其中，的值域为/>，符号/>表示姿态矩阵/>的第/>行第/>列元素。

步骤2：判断俯仰角绝对值<89.9992°时更新航向角/>；否则不更新，默认采用上周期提取的航向角。

根据姿态矩阵的元素更新航向角/>：

；

式中atan2为标准库反正切函数，值域为。

步骤3：判断俯仰角>0°时，通过矩阵元素代数和计算横滚角与航向角之差/>，并提取横滚角/>。

对姿态矩阵的元素进行处理得到：

；

横滚角与航向角之差为：

；

提取横滚角：

；

步骤4：判断俯仰角≤0°时，通过矩阵元素代数和计算横滚角与航向角之和/>，并提取横滚角/>。

对姿态矩阵的元素进行处理能够得到：

；

横滚角与航向角之和为：

；

提取横滚角：

：

步骤5：对横滚角和航向角的取值范围进行规范化。

计算规范化值域为的航向角/>：

；

计算规范化值域为的横滚角/>：

。

下面通过仿真试验对本发明的具体实施方式和实施效果做进一步说明。

仿真条件设置如下：设置载体初始俯仰角、横滚角和航向角为［89.999°; 5°;5°］，以0.002°/s的俯仰角速率向上作抬头运动1s，最终俯仰角越过90°仍停留在89.999°。设置惯导***中陀螺零偏均方差为0.01°/h，随机游走噪声为0.001°/，导航解算采用陀螺角速率每10ms周期进行姿态矩阵更新。

图2a~2c为背景技术通过导航解算姿态矩阵提取的俯仰角、横滚角和航向角。在图2a中0.5s时俯仰角处于90°，受多值性和陀螺微小噪声影响，如图2b、2c所示，该时间段附近横滚角和航向角存在大幅度的不确定跳变。

图3a~3c为本发明技术通过导航解算姿态矩阵提取的俯仰角、横滚角和航向角。图3a中0.5s俯仰角处于90°时，图3b、3c中横滚角和航向角保持正常输出未发生跳变；在0.9s后，当图3a俯仰角越过90°且小于阈值89.9992°时，图3b、3c中横滚角和航向角出现正确的翻转。

实施例效果表明，本发明技术通过计算横滚角与航向角的和或差来提取横滚角，在全姿态范围内均不存在原理性误差；当俯仰角处于±90°附近时，通过不更新航向角有效避免了横滚角的不确定跳变。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims

1.一种避免跳变的惯导***姿态角提取方法，其特征在于：包括以下步骤：

S1、根据惯导***解算姿态矩阵的元素得到俯仰角/>；

姿态矩阵为：

；

俯仰角为：/>；

S4、通过横滚角与航向角之差提取横滚角/>，进入步骤S6；

S5、通过横滚角与航向角之和提取横滚角/>，进入步骤S6

规范化后航向角的值域为/>：

；

规范化后横滚角的值域为/>：

；

一种避免跳变的惯导***姿态角提取方法完成。

2.根据权利要求1所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：步骤S1中，的值域为/>。

3.根据权利要求1所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：步骤S2中，航向角为：

；

其中：atan2为标准库反正切函数，值域为。

4.根据权利要求1所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：步骤S2中，K为89.9992°。

5.根据权利要求1所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：步骤S4中，横滚角为：

。

6.根据权利要求5所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：横滚角与航向角之差为：

。

7.根据权利要求6所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：对姿态矩阵的元素进行作差和求和处理得到：

。

8.根据权利要求1所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：步骤S5中，横滚角为：

。

9.根据权利要求8所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：横滚角与航向角之和为：

。

10.根据权利要求9所述的一种避免跳变的惯导***姿态角提取方法，其特征在于：对姿态矩阵的元素进行作差和求和处理得到：

。