组合导航——欧拉角、姿态阵、四元数的转换关系

liiiuzy

已于 2024-01-08 16:42:12 修改

阅读量840

点赞数 8

分类专栏：组合导航学习文章标签： matlab

于 2024-01-05 00:58:12 首次发布

本文链接：https://blog.csdn.net/liiiuzy/article/details/135396209

版权

组合导航学习专栏收录该内容

3 篇文章 2 订阅

订阅专栏

公式来源于西北工业大学严恭敏老师的《捷联惯导算法与组合导航原理》
代码来自西北工业大学严恭敏老师的PSINS开源代码

文章目录

欧拉角、姿态阵、四元数的转换关系

欧拉角、姿态阵、四元数的转换关系

Alt

说明

常用载体的姿态描述为：航向角北偏东为正（俯视），俯仰角抬头为正，横滚角右横滚为正（后往前看）；
载体欧拉角本质上按物理轴向定义，一般依次按 立轴下->横轴右->纵轴前 进行旋转；

Alt
所以，在选取东北天为地理坐标系，右前上为载体坐标系，按上述进行旋转，旋转顺序为（-3）1 2，即对应航向角，俯仰角、横滚角。

但在导航系为东北天时，航向角旋转方式与右手定则不符，故设航向角北偏西为正，下面所有公式对应的旋转顺序为 312，即航向角北偏西为正，俯仰角抬头正、横滚角右滚正。

欧拉角到姿态阵

计算公式

东北天312下，地理坐标系（这里是导航坐标系n系）到载体坐标系（b系）的方向余弦矩阵
在这里插入图片描述
关于方向余弦阵：这里新手容易混淆一个概念，就是坐标变换与坐标系变换，这两者是相反的，方向余弦阵主要是反映坐标系的变换。

代码

function [Cnb, Cnbr] = a2mat(att)
% Input: att - att=[pitch; roll; yaw] in radians
% 这里输入是[俯仰，横滚，航向],单位是弧度制
% Outputs: Cnb - DCM from navigation-frame(n) to body-frame(b), in yaw->pitch->roll
%                (3-1-2) rotation sequence
%          Cnbr - DCM in yaw->roll->pitch (3-2-1) rotation sequence

%         解释：
%			Cnb表示按3-1-2方式旋转
%		   Cnbr表示按3-2-1方式旋转
    s = sin(att); c = cos(att);
    si = s(1); sj = s(2); sk = s(3); 
    ci = c(1); cj = c(2); ck = c(3);
    Cnb = [ cj*ck-si*sj*sk, -ci*sk,  sj*ck+si*cj*sk;
            cj*sk+si*sj*ck,  ci*ck,  sj*sk-si*cj*ck;
           -ci*sj,           si,     ci*cj           ];
    if nargout==2  % dual Euler angle DCM
        Cnbr = [ cj*ck, si*sj*ck-ci*sk, ci*sj*ck+si*sk;
                 cj*sk, si*sj*sk+ci*ck, ci*sj*sk-si*ck;
                -sj,    si*cj,          ci*cj            ];
    end

姿态阵到欧拉角

欧拉角到四元数

计算公式

Alt
这里的c，s指的是 cos、sin

代码

function qnb = a2qua(att)
% Input: att - att=[pitch; roll; yaw] in radians
% Output: qnb - attitude quaternion
% 这里输入是[俯仰，横滚，航向],单位是弧度制
    att2 = att/2;
    s = sin(att2); c = cos(att2);
    sp = s(1); sr = s(2); sy = s(3); 
    cp = c(1); cr = c(2); cy = c(3); 
    qnb = [ cp*cr*cy - sp*sr*sy;
            sp*cr*cy - cp*sr*sy;
            cp*sr*cy + sp*cr*sy;
            cp*cr*sy + sp*sr*cy ];