惯性导航学习过程记录（旋转矩阵部分）

        坐标系变换经常弄混淆，这里记录一下相关旋转的过程，代码是CSDN上下的，链接找不到了，直接搜索惯性导航程序，然后随便买一个就行了，程序大家写的都差不多。

        最近从CSDN上下载的MATLAB程序都会出现中文显示乱码问题，解决方法：用记事本打开.m文件，重新另存为，格式改成ANSI,然后回到matlab里面就能打开了。

1.绕单轴旋转

1.1 绕x轴旋转

function angleInRadians = deg2rad(angleInDegrees)
% 将角度从度转换为弧度。
%   DEG2RAD(X) converts angle units from degrees to radians for each
%   element of X.
%
%   See also RAD2DEG.% Copyright 2015 The MathWorks, Inc.if isfloat(angleInDegrees)angleInRadians = (pi/180) * angleInDegrees;
elseerror(message('MATLAB:deg2rad:nonFloatInput'))
endfunction Cb2n = ch_rotx(theta)
% 3D初等旋转， theta为旋转角度，rad
Cb2n = [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)];
endtheta = deg2rad(30);
Cb2nX = ch_rotx(theta);
fprintf("b系到n系，绕X轴旋转%.3f ° 的旋转矩阵 Cb2nX 为\n", rad2deg(theta));
Cb2nX

运行结果：

b系到n系，绕X轴旋转30.000 ° 的旋转矩阵 Cb2nX 为

Cb2nX =

        1.0000         0         0
         0    0.8660   -0.5000
         0    0.5000    0.8660

 1.2 绕y轴旋转

function Cb2n = ch_roty(theta)
% 3D鍒濈瓑鏃嬭浆锛� theta涓烘棆杞搴︼紝rad
Cb2n = [cos(theta), 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)];
endtheta = deg2rad(40);
Cb2nY = ch_roty(theta);
fprintf("b系到n系的，绕Y轴旋转%.3f °  的旋转矩阵 Cb2nX为\n", rad2deg(theta));
Cb2nY

运行结果： 

b系到n系的，绕Y轴旋转40.000 °  的旋转矩阵 Cb2nX为

Cb2nY =

    0.7660         0    0.6428
         0    1.0000         0
   -0.6428         0    0.7660

 1.3 绕z轴旋转

function Cb2n = ch_rotz(theta)
% 3D鍒濈瓑鏃嬭浆锛� theta涓烘棆杞搴︼紝rad
Cb2n = [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1];
endtheta = deg2rad(50);
Cb2nZ = ch_rotz(theta);
fprintf("b系到n系的，绕Z轴旋转%.3f °  的旋转矩阵 Cb2nX为\n", rad2deg(theta));
Cb2nZ

运行结果： 

b系到n系的，绕Z轴旋转50.000 °  的旋转矩阵 Cb2nX为

Cb2nZ =

    0.6428   -0.7660         0
    0.7660    0.6428         0
         0         0    1.000

1.4 利用旋转矩阵对一点进行旋转

Ab = [0 0 1]';
fprintf("b系下有点 A:%.3f %.3f %.3f\n", Ab(1), Ab(2), Ab(3));An = Cb2nX*Ab;
fprintf("b系下点A经过Cb2nX旋转到n系为: %.3f %.3f %.3f\n",An(1), An(2), An(3));An = Cb2nY*Ab;
fprintf("b系下点A经过Cb2nY旋转到n系为: %.3f %.3f %.3f\n",An(1), An(2), An(3));An = Cb2nZ*Ab;
fprintf("b系下点A经过Cb2nZ旋转到n系为: %.3f %.3f %.3f\n",An(1), An(2), An(3));

运行结果： 

b系下有点 A:0.000 0.000 1.000
b系下点A经过Cb2nX旋转到n系为: 0.000 -0.500 0.866
b系下点A经过Cb2nY旋转到n系为: 0.643 0.000 0.766
b系下点A经过Cb2nZ旋转到n系为: 0.000 0.000 1.000

 上述重点： 

• 角度需要先经过函数 deg2rad （）化成弧度
• 旋转过程为旋转矩阵*b系下一点的坐标
• 旋转矩阵尺寸是3x3，因此b系下的点坐标为3行1列的格式

2.绕多个轴旋转

2.1  312旋转方式

先绕z轴旋转50度，再绕x轴旋转30度，最后绕y轴旋转40度，也就是312

eul = [30 40 50]';fprintf("按312顺序(先转Z-然后X-最后Y)旋转，其中X,Y,Z旋转角度为%d° %d° %d° 得到对应的坐标变换矩阵：\n", eul(1), eul(2), eul(3));eul_rad = deg2rad(eul);
Cb2n_312 =  ch_rotz(eul_rad(3)) * ch_rotx(eul_rad(1)) * ch_roty(eul_rad(2));
Cb2n_312

运行结果： 

按312顺序(先转Z-然后X-最后Y)旋转，其中X,Y,Z旋转角度为30° 40° 50° 得到对应的坐标变换矩阵：

Cb2n_312 =

    0.2462   -0.6634    0.7066
    0.7934    0.5567    0.2462
   -0.5567    0.5000    0.6634

 2.2  321旋转方式

fprintf("按321顺序(先转Z-然后Y-最后Z)旋转，其中X,Y,Z旋转角度为%d° %d° %d° 得到对应的坐标变换矩阵：\n", eul(1), eul(2), eul(3));Cb2n_321 =  ch_rotz(eul_rad(3)) * ch_roty(eul_rad(2)) * ch_rotx(eul_rad(1));
Cb2n_321

 运行结果：

按321顺序(先转Z-然后Y-最后Z)旋转，其中X,Y,Z旋转角度为30° 40° 50° 得到对应的坐标变换矩阵：

Cb2n_321 =

    0.4924   -0.4568    0.7408
    0.5868    0.8029    0.1050
   -0.6428    0.3830    0.6634

旋转方式不同得到的旋转矩阵也不相同！

2.3   b系转n系-n系转b系

fprintf("从n系到b系的坐标变换矩阵Cn2b:(既Cb2n的转置)\n");
Cn2b_312 = Cb2n_312';
Cn2b_312

运行结果：

从n系到b系的坐标变换矩阵Cn2b:(既Cb2n的转置)

Cn2b_312 =

    0.2462    0.7934   -0.5567
   -0.6634    0.5567    0.5000
    0.7066    0.2462    0.6634

 n转到b,是Cn2b,注意角标，n to b

2.4  欧拉角反求Cn2b

fprintf("Cn2b也可以通过欧拉角计算:， 注意矩阵的旋转顺序 和 欧拉角 符号都要变\n");
Cn2b_312 = ch_roty(-eul_rad(2))  * ch_rotx(-eul_rad(1)) * ch_rotz(-eul_rad(3));
Cn2b_312

运行结果：

Cn2b也可以通过欧拉角计算:， 注意矩阵的旋转顺序 和 欧拉角 符号都要变

Cn2b_312 =

    0.2462    0.7934   -0.5567
   -0.6634    0.5567    0.5000
    0.7066    0.2462    0.6634

 快捷记法：312 倒过来 - 213

3. 从姿态角 pitch, roll, yaw 到旋转矩阵

pitch,roll,yaw绕哪一个轴并不固定，每个地方的习惯不同

 pitch - 俯仰角 - 绕y轴

 roll - 横滚角 - 绕x轴

 yaw - 航向角 - 绕z轴（有的地方也写heading）

3.1 pitch 是x轴的情况，312

function [Cb2n_312, Cb2n_321] = ch_eul2m(att)
% 将欧拉角转换为姿态阵
% 复用严龚敏老师的a2mat
%
% Input: att  单位：rad
% 对于312((Z->X->Y))顺序，对应att = [pitch(绕X轴) roll(绕Y轴)  yaw(绕Z轴)]
% 对于321(Z->Y->X)顺序，对应att = [roll(绕X轴) pitch(绕Y轴)  yaw(绕Z轴)]
% Outputs: 
% Cb2n_312:  312欧拉角顺序下转换后的Cb2n
% Cb2n_321:  321欧拉角顺序下转换后的Cb2ns = sin(att); c = cos(att);si = s(1); sj = s(2); sk = s(3); ci = c(1); cj = c(2); ck = c(3);Cb2n_312 = [ 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 DCMCb2n_321 = [ 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            ];endfunction [eul_312, eul_321] = ch_m2eul(Cb2n)
% 将姿态阵转为欧拉角
% 复用严龚敏老师的m2att
%
% Input: 姿态阵Cb2n: b系->n系的坐标变换矩阵
% Outputs: 
% eul_312:   312(Z->X->Y)旋转顺序下的欧拉角:  att = [pitch(绕X轴) roll(绕Y轴) yaw(绕Z轴)]
% eul_321:   321(Z->Y->X)旋转顺序下的欧拉角:  att = [roll(绕X轴) pitch(绕Y轴)  yaw(绕Z轴)]eul_312 = [ asin(Cb2n(3,2));atan2(-Cb2n(3,1),Cb2n(3,3)); atan2(-Cb2n(1,2),Cb2n(2,2)) ];if nargout==2  % dual Euler angleseul_321 = [ atan2(Cb2n(3,2),Cb2n(3,3)); asin(-Cb2n(3,1)); atan2(Cb2n(2,1),Cb2n(1,1)) ];endeul_312 = [30, 40, 50]; % [pitch(绕X轴) roll(绕Y轴)  yaw(绕Z轴)]fprintf("按312顺序(先转Z-然后X-最后Y)旋转，其中X,Y,Z旋转角度为%.3f° %.3f° %.3f° 得到对应的坐标变换矩阵：\n", eul_312(1), eul_312(2), eul_312(3));eul_312rad = deg2rad(eul_312);
[Cb2n_312, ~] = ch_eul2m(eul_312rad);
Cb2n_312

运行结果：

按312顺序(先转Z-然后X-最后Y)旋转，其中X,Y,Z旋转角度为30.000° 40.000° 50.000° 得到对应的坐标变换矩阵：

Cb2n_312 =

    0.2462   -0.6634    0.7066
    0.7934    0.5567    0.2462
   -0.5567    0.5000    0.6634

将Cb2n_312转回欧拉角:
坐标变换矩阵转欧拉角:30.000°(Pitch) 40.000°(Roll) 50.000°(Yaw)

 3.2 roll 是x轴的情况，321

eul_321 = [30, 40, 50]; % [roll(绕X轴) pitch(绕Y轴)  yaw(绕Z轴)]fprintf("\n按321顺序(先转Z-然后Y-最后X)旋转，其中X,Y,Z旋转角度为%.3f° %.3f° %.3f° 得到对应的坐标变换矩阵：\n", eul_321(1), eul_321(2), eul_321(3));eul_321rad = deg2rad(eul_321);
[~, Cb2n_321] = ch_eul2m(eul_321rad);
Cb2n_321fprintf("将Cb2n_321转回欧拉角:\n");
[~, eul_321]  = ch_m2eul(Cb2n_321);
eul_321 = rad2deg(eul_321);fprintf("坐标变换矩阵转欧拉角:%.3f°(Roll) %.3f°(Pitch) %.3f°(Yaw)\n", eul_321(1), eul_321(2), eul_321(3));

运行结果：

按321顺序(先转Z-然后Y-最后X)旋转，其中X,Y,Z旋转角度为30.000° 40.000° 50.000° 得到对应的坐标变换矩阵：

Cb2n_321 =

    0.4924   -0.4568    0.7408
    0.5868    0.8029    0.1050
   -0.6428    0.3830    0.6634

将Cb2n_321转回欧拉角:
坐标变换矩阵转欧拉角:30.000°(Roll) 40.000°(Pitch) 50.000°(Yaw)

 反正就是旋转顺序永远是，yaw ,pitch,roll 

4. 四元数与等效旋转矢量

4.1 利用旋转矩阵实现向量的不同坐标系转化

function q = ch_qmul(q1, q2) 
% 四元数相乘
%
% Inputs: Q1 Q2, 四元数和矩阵一样，不满足交换律
% Outputs: Q
%   q = [ q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4);q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3);q1(1) * q2(3) + q1(3) * q2(1) + q1(4) * q2(2) - q1(2) * q2(4);q1(1) * q2(4) + q1(4) * q2(1) + q1(2) * q2(3) - q1(3) * q2(2) ];function Qb2n = ch_m2q(Cb2n)
% 姿态阵转四元数
%
% Input: Cb2n
% Output: Qb2n
%C11 = Cb2n(1,1); C12 = Cb2n(1,2); C13 = Cb2n(1,3); C21 = Cb2n(2,1); C22 = Cb2n(2,2); C23 = Cb2n(2,3); C31 = Cb2n(3,1); C32 = Cb2n(3,2); C33 = Cb2n(3,3); if C11>=C22+C33q1 = 0.5*sqrt(1+C11-C22-C33);q0 = (C32-C23)/(4*q1); q2 = (C12+C21)/(4*q1); q3 = (C13+C31)/(4*q1);elseif C22>=C11+C33q2 = 0.5*sqrt(1-C11+C22-C33);q0 = (C13-C31)/(4*q2); q1 = (C12+C21)/(4*q2); q3 = (C23+C32)/(4*q2);elseif C33>=C11+C22q3 = 0.5*sqrt(1-C11-C22+C33);q0 = (C21-C12)/(4*q3); q1 = (C13+C31)/(4*q3); q2 = (C23+C32)/(4*q3);elseq0 = 0.5*sqrt(1+C11+C22+C33);q1 = (C32-C23)/(4*q0); q2 = (C13-C31)/(4*q0); q3 = (C21-C12)/(4*q0);endQb2n = [q0; q1; q2; q3];function Cb2n = ch_q2m(Qb2n)
% 四元数转姿态阵
%
% Input: Qb2n
% Output: Cb2n
%q11 = Qb2n(1)*Qb2n(1); q12 = Qb2n(1)*Qb2n(2); q13 = Qb2n(1)*Qb2n(3); q14 = Qb2n(1)*Qb2n(4); q22 = Qb2n(2)*Qb2n(2); q23 = Qb2n(2)*Qb2n(3); q24 = Qb2n(2)*Qb2n(4);     q33 = Qb2n(3)*Qb2n(3); q34 = Qb2n(3)*Qb2n(4);  q44 = Qb2n(4)*Qb2n(4);Cb2n = [ q11+q22-q33-q44,  2*(q23-q14),     2*(q24+q13);2*(q23+q14),      q11-q22+q33-q44, 2*(q34-q12);2*(q24-q13),      2*(q34+q12),     q11-q22-q33+q44 ];function qout = ch_qconj(qin) 
qout = [qin(1); -qin(2:4)];function vo = ch_qmulv(q, vi)
% 向量通过四元数做3D旋转
% 
% Inputs: q - Qb2n
%              vi - 需要旋转的向量
% Output: vout - output vector, such that vout = q*vin*conjugation(q)
% 
% See also  q2mat, qconj, qmul.%     qi = [0; vi];
%     qo = ch_qmul(ch_qmul(q,qi),ch_qconj(q));
%     vo = qo(2:4,1);qo1 =              - q(2) * vi(1) - q(3) * vi(2) - q(4) * vi(3);qo2 = q(1) * vi(1)                + q(3) * vi(3) - q(4) * vi(2);qo3 = q(1) * vi(2)                + q(4) * vi(1) - q(2) * vi(3);qo4 = q(1) * vi(3)                + q(2) * vi(2) - q(3) * vi(1);vo = vi;vo(1) = -qo1 * q(2) + qo2 * q(1) - qo3 * q(4) + qo4 * q(3);vo(2) = -qo1 * q(3) + qo3 * q(1) - qo4 * q(2) + qo2 * q(4);vo(3) = -qo1 * q(4) + qo4 * q(1) - qo2 * q(3) + qo3 * q(2);

clear;
clc
close all;%% 已知
Vn = [0 0 1];
Qa2n = [0.981 0.010 -0.191 0.011]';   % a系转到n系的四元数
Qb2n = [0.936 -0.259 -0.233 -0.046]';   % b系转到n系的四元数Va = [0.375 0.021 0.876]';    % a 系上的一点
Vb = [0.465  -0.447 0.712]';  % b 系上的一点%% 四元数转成矩阵
Ca2n = ch_q2m(Qa2n);       % a系转到n系的旋转矩阵
Cb2n = ch_q2m(Qb2n);        % b系转到n系的旋转矩阵%%计算 Ca2b, Qa2b
Ca2b = Cb2n' * Ca2n;   % a系 转到 b系 =  先 a系到 n系，然后n 系到b系（用b系转到n系的旋转矩阵的转置求）
Qa2b = ch_m2q(Ca2b);%% 转换并显示结果
fprintf("a2b的四元数为:%.3f %.3f %.3f %.3f\n", Qa2b(1), Qa2b(2), Qa2b(3), Qa2b(4));
Vb_ = Ca2b*Va;
fprintf("通过Ca2b 将Va转成Vb: %.3f %.3f %.3f, 约等于: %.3f %.3f %.3f \n", Vb_(1), Vb_(2), Vb_(3), Vb(1), Vb(2), Vb(3));

 运行结果：

a2b的四元数为:0.960 0.275 0.047 0.004
通过Ca2b 将Va转成Vb: 0.455 -0.432 0.716, 约等于: 0.465 -0.447 0.712 

4.2  利用四元数实现向量的不同坐标系转化

Qa2b = ch_qmul(ch_qconj(Qb2n), Qa2n);
Vb_ = ch_qmulv(Qa2b, Va);fprintf("通过Qa2b 将Va转成Vb: %.3f %.3f %.3f, 约等于: %.3f %.3f %.3f \n", Vb_(1), Vb_(2), Vb_(3), Vb(1), Vb(2), Vb(3));Qb2a = ch_qconj(Qa2b);Va_ = ch_qmulv(Qb2a, Vb);fprintf("通过Qb2a 将Vb转成Va: %.3f %.3f %.3f, 约等于: %.3f %.3f %.3f \n", Va_(1), Va_(2), Va_(3), Va(1), Va(2), Va(3));

运行结果：

通过Qa2b 将Va转成Vb: 0.455 -0.432 0.716, 约等于: 0.465 -0.447 0.712 
通过Qb2a 将Vb转成Va: 0.384 0.006 0.879, 约等于: 0.375 0.021 0.876 

  a系 转到 b系 =  先a系到n系，然后n系到b系

• n系到b系的旋转矩阵 = b系转到n系的旋转矩阵的转置
• n系到b系的四元数 = b系转到n系的四元数取负，后三位取负

然后就是旋转矩阵相乘，或是四元数相乘，这里需要注意顺序： b>n * a>n  = a>n