前言
文中算法公式摘自《捷联惯导算法与组合导航原理》(严恭敏、翁浚 编著)、《惯性导航》(秦永元 编著),其他理解仅代表个人观点。本文是对姿态角和姿态矩阵之间转化的理解。
一、定义
机体坐标系b系定义:
o飞机质心
x轴指向机头方向
z轴处在飞机对称面垂直x轴向下
y轴垂直oxz平面指向飞机右侧
导航系n系的定义:
xyz轴指向北东地(或东北天)方向
姿态角的定义(这里地面系指北东地坐标):
俯仰角:机体系x轴与地平面夹角,俯仰角抬头为正
横滚角:机体系z轴与包含机体系x轴铅锤面的夹角,右滚为正
航向角:机体系x轴在地平面投影与地面系x轴的夹角,投影在地面系x轴右侧为正。
二、姿态角转化为姿态矩阵
导航系转化为机体系:向右旋转航向角、向上旋转俯仰角、向右旋转横滚角,该顺序旋转符合姿态角的定义。
先用北东地坐标系
![](https://i-blog.csdnimg.cn/blog_migrate/465bcd606ff5cd0e80f0231e605a7251.png)
转化到三维坐标
![](https://i-blog.csdnimg.cn/blog_migrate/7d5ec0615b2ad68f4650830eb8bd4798.png)
![](https://i-blog.csdnimg.cn/blog_migrate/a3113f2dfd7fec1845cd80ae717a9fe2.png)
![](https://i-blog.csdnimg.cn/blog_migrate/8b0b94fca83686cc5b88727d9c216224.png)
采用北东地坐标系时:先绕z轴转航向角,再绕y轴转俯仰角,最后绕x轴转横滚角
![](https://i-blog.csdnimg.cn/blog_migrate/6af6b52a35a896cd920adb9a7cdb69e2.png)
采用东北天坐标系时(机体系右前上,航向角北偏东为正时):先绕z轴转航向角,再绕x轴转俯仰角,最后绕y轴转横滚角。
如果航向角北偏西为正,取负即可。
![](https://i-blog.csdnimg.cn/blog_migrate/ddd4784cdb16771e409eb0518d3b7198.png)
三、根据姿态矩阵求姿态角
根据姿态矩阵中的角度求解姿态角。
之前程序中由于俯仰角范围在-pi/2~pi/2 范围内,余弦值为正,其实并不完全合理。
根据余弦项可以求出俯仰角,可以知道俯仰角的正负,因此可以判断出横滚和航向角正余弦
的符号,根据四象限反正切,可以求出横滚和航向角。