一种避免跳变的惯导系统姿态角提取方法与流程-k8凯发

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

背景技术：

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

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

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

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

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

技术实现思路

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

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

3、s1、根据惯导系统解算姿态矩阵的元素得到俯仰角；

4、姿态矩阵为：

5、；

6、俯仰角为：；

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

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

9、s3、判断俯仰角是否大于零，如果是，进入步骤s4；如果否，进入步骤s5；

10、s4、通过横滚角与航向角之差提取横滚角，进入步骤s6；

11、s5、通过横滚角与航向角之和提取横滚角，进入步骤s6;

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

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

14、规范化后横滚角的值域为[，)：

15、；

16、一种避免跳变的惯导系统姿态角提取方法完成。

17、本发明所述的一种避免跳变的惯导系统姿态角提取方法，作为优选方式，步骤s1中，的值域为[，)。

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

19、；

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

21、本发明所述的一种避免跳变的惯导系统姿态角提取方法，作为优选方式，步骤s2中，k为89.9992°。

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

23、。

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

25、。

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

27、。

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

29、。

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

31、。

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

33、。

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

35、姿态矩阵的构成为：

36、；

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

38、；    （1）

39、；    （2）

40、；     （3）

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

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

43、；

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

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

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

47、；   （4）

48、；   （5）

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

50、；     （6）

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

52、；    （7）

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

54、根据式（4）可得：

55、；

56、根据式（5）可得：

57、；

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

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

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

61、本发明具有以下优点：

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