坐标变换学习笔记—代码篇Matlab

最新推荐文章于 2025-08-04 18:39:53 发布

原创最新推荐文章于 2025-08-04 18:39:53 发布 · 6.2k 阅读

43 ·

CC 4.0 BY-SA版权

SLAM 专栏收录该内容

24 篇文章

订阅专栏

本文详细介绍了在Matlab中如何进行四元数、旋转矩阵和欧拉角之间的转换。通过分析不同函数如quat2dcm、quat2angle、dcm2quat等，探讨了转换的数学原理和Matlab实现。此外，文章指出在Matlab中，欧拉角顺序通常为yaw, pitch, roll，且推荐使用eul, rotm系列函数，以保持与ROS的一致性。同时，注意到Matlab中四元数的顺序是w,x,y,z，这与Eigen相同，但与ROS相反。" 111576689,10293598,Spring框架中的设计模式详解,"['Spring框架', '设计模式', 'AOP', 'DI', '适配器']

坐标变换学习笔记—代码篇Matlab

在Matlab中，旋转量间的变换类不止一个，在 “C:\Program Files\Polyspace\R2020a\toolbox\aero\aero\” 和 “C:\Program Files\Polyspace\R2020a\mcr\toolbox\aero\aero\” 两个目录下均有关于四元数，旋转矩阵，欧拉角间变换的函数文件：

四元数 $\to$ 旋转矩阵：quat2dcm.m （×，得到式 $(2.1.2)$ 对应矩阵的转置）
四元数 $\to$ 欧拉角：quat2angle.m （√）
旋转矩阵 $\to$ 四元数：dcm2quat.m（√）
旋转矩阵 $\to$ 欧拉角：dcm2angle.m（×）
欧拉角 $\to$ 旋转矩阵：angle2dcm.m（×）
欧拉角 $\to$ 四元数：angle2quat.m（√）
而在 “C:\Program Files\Polyspace\R2020a\mcr\toolbox\shared\robotics\robotcore” 目录中，也有这些旋转量之间的转换函数(下面的旋转向量，在Matlab中不展开讨论)：
旋转向量 $\to$ 四元数：axang2quat.m （√）
旋转向量 $\to$ 旋转矩阵：axang2rotm.m （看不太明白）
四元数 $\to$ 旋转向量：quat2axang.m （√）
四元数 $\to$ 旋转矩阵：quat2rotm.m （√）
四元数 $\to$ 欧拉角：quat2eul.m （√）
旋转矩阵 $\to$ 旋转向量：rotm2axang.m （√）
旋转矩阵 $\to$ 四元数：rotm2quat.m (new method)
旋转矩阵 $\to$ 欧拉角：rotm2eul.m (√)
欧拉角 $\to$ 旋转矩阵：eul2rotm.m （√）
欧拉角 $\to$ 四元数：eul2quat.m （√）
在Matlab中，旋转矩阵，四元数，欧拉角均没有什么特定类型，四元数可用一个1x4的向量表示，旋转矩阵可用3x3的矩阵表示。与Eigen和ROS相比Matlab的优势在于它可以批量将四元数组成的数组批量转换成旋转矩阵或欧拉角数组。

axang2rotm.m中旋转向量转旋转矩阵的实现方式：

% For a single axis-angle vector [ax ay az theta] the output rotation
% matrix R can be computed as follows:
% R = [txx + c txy - zs txz + ys
% txy + zs tyy + c tyz - xs
% txz - ys tyz + xs tzz + c]
% where,
% c = cos(theta)
% s = sin(theta)
% t = 1 - c
% x = normalized axis ax coordinate
% y = normalized axis ay coordinate
% z = normalized axis az coordinate

rotm2axang.m中，旋转矩阵转旋转向量实现方式如下：

theta = real(acos(complex((1/2)(R(1,1,:)+R(2,2,:)+R(3,3,:)-1))));

% Determine initial axis vectors from theta
v = [ R(3,2,:)-R(2,3,:),…
R(1,3,:)-R(3,1,:),…
R(2,1,:)-R(1,2,:)] ./ (repmat(2sin(theta),[1,3]));

实际上上面是基于理论篇的式 $(1.2)$ 求欧拉角，式 $(2.1) 和 (3.2)$ 来求旋转轴，即：
$\frac{[4q_0q_1~4q_0q_2~4q_0q_3]^T}{4q_0 sin(\frac{\theta}{2})} = \frac{[4q_0q_1~4q_0q_2~4q_0q_3]^T}{2sin(\theta)}$

Matlab这篇总结的稍微有点粗糙，将就看吧。先说本篇博客基本结论：
（1）在Matlab中，关于旋转量间的变换，强烈建议使用eul, rotm这一类的转换函数，它们默认的欧拉角顺序是ZYX，与ROS中的顺序是一致的，而且符合大多数情况下默认使用方式。
（2）注意点1，Matlab中的欧拉角，不管是eul还是angle相关的方法，对应输入输出的rpy顺序均是：yaw, pitch, roll
（3）注意点2，Matlab中使用的四元数顺序是：w,x,y,z，与Eigen使用的四元数顺序一致(w,x,y,z)，与ROS使用的四元数顺序相反(x,y,z,w)。

四元数 $\to$ 旋转矩阵

quat2dcm

quat2dcm.m 中的 m = quat2dcm( q ) 函数用于将四元数转换成旋转矩阵，这里的输入参数q是Mx4的向量，表示M个四元数，而返回值m是一个3x3xM的矩阵，表示M个对应的旋转矩阵。注意，在Matlab中，四元数的顺序是w,x,y,z 。

function dcm = quat2dcm( q )
%  QUAT2DCM Convert quaternion to direction cosine matrix.
%   N = QUAT2DCM( Q ) calculates the direction cosine matrix, N, for a
%   given quaternion, Q.  Input Q is an M-by-4 matrix containing M
%   quaternions.  N returns a 3-by-3-by-M matrix of direction cosine 
%   matrices.  The direction cosine matrix performs the coordinate
%   transformation of a vector in inertial axes to a vector in body axes.
%   Each element of Q must be a real number.  Additionally, Q has its
%   scalar number as the first column. 
%
%   Examples:
%
%   Determine the direction cosine matrix from q = [1 0 1 0]:
%      dcm = quat2dcm([1 0 1 0])
%
%   Determine the direction cosine matrices from multiple quaternions:
%      q = [1 0 1 0; 1 0.5 0.3 0.1];
%      dcm = quat2dcm(q)
%
%   See also ANGLE2DCM, DCM2ANGLE, DCM2QUAT, ANGLE2QUAT, QUAT2ANGLE, QUATROTATE. 

%   Copyright 2000-2007 The MathWorks, Inc.

qin = quatnormalize( q );  %% quatnormalize 将多维四元数归一化；

dcm = zeros(3,3,size(qin,1));  %% 预先分配内存，加快运行速度；

dcm(1,1,:) = qin(:,1).^2 + qin(:,2).^2 - qin(:,3).^2 - qin(:,4).^2;  %% q_w^2 + q_x^2 - q_y^2 - q_z^2;
dcm(1,2,:) = 2.*(qin(:,2).*qin(:,3) + qin(:,1).*qin(:,4));           %% 2(q_xq_y + q_wq_z);
dcm(1,3,:) = 2.*(qin(:,2).*qin(:,4) - qin(:,1).*qin(:,3));           %% 2(q_xq_z - q_wq_y);
dcm(2,1,:) = 2.*(qin(:,2).*qin(:,3) - qin(:,1).*qin(:,4));           %% 2(q_xq_y - q_wq_z);
dcm(2,2,:) = qin(:,1).^2 - qin(:,2).^2 + qin(:,3).^2 - qin(:,4).^2;  %% q_w^2 - q_x^2 + q_y^2 - q_z^2;
dcm(2,3,:) = 2.*(qin(:,3).*qin(:,4) + qin(:,1).*qin(:,2));           %% 2(q_yq_z + q_wq_x);
dcm(3,1,:) = 2.*(qin(:,2).*qin(:,4) + qin(:,1).*qin(:,3));           %% 2(q_xq_z + q_wq_y);
dcm(3,2,:) = 2.*(qin(:,3).*qin(:,4) - qin(:,1).*qin(:,2));           %% 2(q_yq_z - q_wq_x);
dcm(3,3,:) = qin(:,1).^2 - qin(:,2).^2 - qin(:,3).^2 + qin(:,4).^2;  %% q_w^2 - q_x^2 - q_y^2 + q_z^2;

从上述代码实现可以看到，这里四元数转换旋转矩阵与理论篇中的式 $(2.1.2)$ 基本一致。但是，得到的旋转矩阵是 $(2.1.2)$ 对应旋转矩阵的转置，这里需要注意。

quat2rotm

quat2rotm.m中的quat2rotm函数，输入q为Nx4的四元数，返回值为3x3xN的N维旋转矩阵。q的顺序是：w,x,y,z。

function R = quat2rotm( q )
%QUAT2ROTM Convert quaternion to rotation matrix
%   R = QUAT2ROTM(QOBJ) converts a quaternion object, QOBJ, into an orthonormal
%   rotation matrix, R. Each quaternion represents a 3D rotation. 
%   QOBJ is an N-element vector of quaternion objects.
%   The output, R, is an 3-by-3-by-N matrix containing N rotation matrices.
%
%   R = QUAT2ROTM(Q) converts a unit quaternion, Q, into an orthonormal
%   rotation matrix, R. The input, Q, is an N-by-4 matrix containing N quaternions. 
%   Each quaternion represents a 3D rotation and is of the form q = [w x y z], 
%   with w as the scalar number. Each element of Q must be a real number.
%
%   Example:
%      % Convert a quaternion to rotation matrix
%      q = [0.7071 0.7071 0 0];
%      R = quat2rotm(q)
%
%      % Convert a quaternion object
%      qobj = quaternion([sqrt(2)/2 0 sqrt(2)/2 0]);
%      R = quat2rotm(qobj);
%
%   See also rotm2quat, quaternion

%   Copyright 2014-2018 The MathWorks, Inc.

%#codegen

% Validate the quaternions
q = robotics.internal.validation.validateQuaternion(q, 'quat2rotm', 'q');

% Normalize and transpose the quaternions
q = robotics.internal.normalizeRows(q).';

% Reshape the quaternions in the depth dimension
q2 = reshape(q,[4 1 size(q,2)]);

s = q2(1,1,:);
x = q2(2,1,:);
y = q2(3,1,:);
z = q2(4,1,:);

% Explicitly define concatenation dimension for codegen
tempR = cat(1, 1 - 2*(y.^2 + z.^2),   2*(x.*y - s.*z),   2*(x.*z + s.*y),...
2*(x.*y + s.*z), 1 - 2*(x.^2 + z.^2),   2*(y.*z - s.*x),...
2*(x.*z - s.*y),   2*(y.*z + s.*x), 1 - 2*(x.^2 + y.^2) );

R = reshape(tempR, [3, 3, length(s)]);
R = permute(R, [2 1 3]);

end

quat2rotm中的代码实现与里四元数转换旋转矩阵与理论篇中的式 $(2.1.2)$ 完全一致。

注意： Matlab中四元数顺序式(w,x,y,z)与Eigen中的四元数顺序(w,x,y,z)一致，它们与ROS中的四元数顺序(x,y,z,w)相反。

四元数 $\to$ 欧拉角

quat2angle

quat2angle.m 中的 [r1, r2, r3] = quat2dcm( q ) 函数用于将四元数转换成欧拉角，这里的输入参数q是Mx4的向量，表示M个四元数，而返回值r1, r2, r3则是M维的数组，表示从四元数转换回来的欧拉角。quat2dcm(q, s) 函数中的s表示对应的欧拉角的旋转顺序，它的值为： $^{'} Z Y X^{'},^{'} Z Y Z^{'},^{'} Z X Y^{'},^{'} Z X Z^{'},^{'} Y X Z^{'},^{'} Y X Y^{'},^{'} Y Z X^{'},^{'} Y Z Y^{'},^{'} X Y Z^{'},^{'} X Y X^{'},^{'} X Z Y^{'},^{'} X Z X^{'} .$ 中的一个（默认值为：ZYX），代表依次按照该轴的顺序旋转。（目前对为什么有ZYZ, YZY, XYX,…, 这种某个轴转两次的情况不太清楚（欢迎知道原因的大佬解答），存疑，后面知道再补充）。
对于顺序： $^{'} Z Y X^{'},^{'} Z X Y^{'},^{'} Y X Z^{'},^{'} Y Z X^{'},^{'} X Y Z^{'},^{'} X Z Y^{'}$ ，r1,r3角度范围为 $-\pi \thicksim \pi$ ；而r2的范围为： $-\frac{\pi}{2} \thicksim \frac{\pi}{2}$ 。
对于顺序： $^{'} Z Y Z^{'},^{'} Z X Z^{'},^{'} Y X Y^{'},^{'} Y Z Y^{'},^{'} X Y X^{'},^{'} X Z X^{'}$ ，r1,r3角度范围为 $-\pi \thicksim \pi$ ；而r2的范围为： $\thicksim\pi$ 。
下面我们重点关注我们常用的 $^{'} Z Y X^{'}$ 顺序的转换部分：

function [r1,r2,r3] = quat2angle( q, varargin )
%  QUAT2ANGLE Convert quaternion to rotation angles.
%   [R1, R2, R3] = QUAT2ANGLE( Q ) calculates the calculates the set of
%   rotation angles, R1, R2, R3, for a given quaternion, Q.  Input Q is an
%   M-by-4 matrix containing M quaternions.  R1 returns an M array of
%   first rotation angles.  R2 returns an M array of second rotation
%   angles.  R3 returns an M array of third rotation angles. Each element
%   of Q must be a real number.  Additionally, Q has its scalar number as 
%   the first column. Rotation angles are output in radians.   
%
%   [R1, R2, R3] = QUAT2ANGLE( Q, S ) calculates the set of rotation
%   angles, R1, R2, R3, for a given quaternion, Q, and a
%   specified rotation sequence, S. 
%
%   The default rotation sequence is 'ZYX' where the order of rotation
%   angles for the default rotation are R1 = Z Axis Rotation, R2 = Y Axis
%   Rotation, and R3 = X Axis Rotation. 
%
%   All rotation sequences, S, are supported: 'ZYX', 'ZYZ', 'ZXY', 'ZXZ',
%   'YXZ', 'YXY', 'YZX', 'YZY', 'XYZ', 'XYX', 'XZY', and 'XZX'.
%
%   Examples:
%
%   Determine the rotation angles from q = [1 0 1 0]:
%      [yaw, pitch, roll] = quat2angle( [1 0 1 0] )
%
%   Determine the rotation angles from multiple quaternions:
%      q = [1 0 1 0; 1 0.5 0.3 0.1];
%      [pitch, roll, yaw] = quat2angle( q, 'YXZ' )
%
%   See also ANGLE2DCM, DCM2ANGLE, DCM2QUAT, ANGLE2QUAT, QUAT2DCM. 

%   Copyright 2000-2018 The MathWorks, Inc.

%   Limitations: 
%   The limitations for the 'ZYX', 'ZXY', 'YXZ', 'YZX', 'XYZ', and 'XZY'
%   implementations generate an R2 angle that lies between +/- 90 
%   degrees, and R1 and R3 angles that lie between +/- 180 degrees. 
%
%   The limitations for the 'ZYZ', 'ZXZ', 'YXY', 'YZY', 'XYX', and 'XZX'
%   implementations generate an R2 angle that lies between 0 and 
%   180 degrees, and R1 and R3 angles that lie between +/- 180 degrees. 

narginchk(1, 2);

if nargin == 1
    type = 'zyx';
else
    if ischar( varargin{1} ) || isstring( varargin{1} )
        type = char(varargin{1});
    else
        error(message('aero:quat2angle:notChar'));
    end
end

qin = quatnormalize( q );

switch lower( type )
    case 'zyx'
        [r1,r2,r3] = threeaxisrot( 2.*(qin(:,2).*qin(:,3) + qin(:,1).*qin(:,4)), ...
                                   qin(:,1).^2 + qin(:,2).^2 - qin(:,3).^2 - qin(:,4).^2, ...
                                  -2.*(qin(:,2).*qin(:,4) - qin(:,1).*qin(:,3)), ...
                                   2.*(qin(:,3).*qin(:,4) + qin(:,1).*qin(:,2)), ...
                                   qin(:,1).^2 - qin(:,2).^2 - qin(:,3).^2 + qin(:,4).^2);
    case 'zyz'
        [r1,r2,r3] = twoaxisrot( 2.*(qin(:,3).*qin(:,4) - qin(:,1).*qin(:,2)), ...
                                 2.*(qin(:,2).*qin(:,4) + qin(:,1).*qin(:,3)), ...
                                 qin(:,1).^2 - qin(:,2).^2 - qin(:,3).^2 + qin(:,4).^2, ...
                                 2.*(qin(:,3).*qin(:,4) + qin(:,1).*qin(:,2)), ...
                                -2.*(qin(:,2).*qin(:,4) - qin(:,1).*qin(:,3)));
                
    case 'zxy'
        [r1,r2,r3] = threeaxisrot( -2.*(qin(:,2).*qin(:,3) - qin(:,1).*qin(:,4)), ...
                                    qin(:,1).^2 - qin(:,2).^2 + qin(:,3).^2 - qin(:,4).^2, ...
                                    2.*(qin(:,3).*qin(:,4) + qin(:,1).*qin(:,2)), ...
                                   -2.*(qin(:,2).*qin(:,4) - qin(:,1).*qin(:,3)), ...
                                    qin(:,1).^2 - qin(:,2).^2 - qin(:,3).^2 + qin(:,4).^2);

    case 'zxz'
        [r1,r2,r3] = twoaxisrot( 2.*(qin(:,2).*qin(:,4) + qin(:,1).*qin(:,3)), ...
                                -2.*(qin(:,3).*qin(:,4) - qin(:,1).*qin(:,2)), ...
                                 qin(:,1).^2 - qin(:,2).^2 - qin(:,3).^2 + qin(:,4).^2, ...
                                 2.*(qin(:,2).*qin(:,4) - qin(:,1).*qin(:,3)), ...
                                 2.*(qin(:,3).*qin(:,4) + qin(:,1).*qin(:,2)));

    case 'yxz'
        [r1,r2,r3] = threeaxisrot( 2.*(qin(:,2).*qin(:,4) + qin(:,1).*qin(:,3)), ...
                                   qin(:,1).^2 - qin(:,2).^2 - qin(:,3).^2 + qin(:,4).^2, ...
                                  -2.*(qin(:,3).*qin(:,4) - qin(:,1).*qin(:,2)), ...
                                   2.*(qin(:,2).*qin(:,3) + qin(:,1).*qin(:,4)), ...
                                   qin(:,1).^2 - qin(:,2).^2 + qin(:,3).^2 - qin(:,4).^2);
       
    case 'yxy'
        [r1,r2,r3] = twoaxisrot( 2.*(qin(:,2).*qin(:,3) - qin(:,1).*qin(:,4)), ...
                                 2.*(qin(:,3).*qin(:,4) + qin(:,1).*qin(:,2)), ...
                                 qin(:,1).^2 - qin(:,2).^2 + qin(:,3).^2 - qin(:,4).^2, ...
                                 2.*(qin(:,2).*qin(:,3) + qin(:,1).*qin(:,4)), ...
                                -2.*(qin(:,3).*qin(:,4) - qin(:,1).*qin(:,2)));
      
    case 'yzx'       
        [r1,r2,r3] = threeaxisrot( -2.*(qin(:,2).*qin(:,4) - qin(:,1).*qin(:,3)), ...
                                    qin(:,1).^2 + qin(:,2).^2 - qin(:,3).^2 - qin(:,4).^2, ...
                                    2.*(qin(:,2).*qin(:,3) + qin(:,1).*qin(:,4)), ...
                                   -2.*(qin(:,3).*qin(:,4) - qin(:,1).*qin(:,2)), ...
                                    qin(:,1).^2 - qin(:,2).^2 + qin(:,3).^2 - qin(:,4).^2);
        
    case 'yzy'
        [r1,r2,r3] = twoaxisrot( 2.*(qin(:,3).*qin(:,4) + qin(:,1).*qin(:,2)), ...
                                -2.*(qin(:,2).*qin(:,3) - qin(:,1).*qin(:,4)), ...
                                 qin(:,1).^2 - qin(:,2).^2 + qin(:,3).^2 - qin(:,4).^2, ...
                                 2.*(qin(:,3).*qin(:,4) - qin(:,1).*qin(:,2)), ...
                                 2.*(qin(:,2).*qin(:,3) + qin(:,1).*qin(:,4)));

    case 'xyz'
        [r1,r2,r3] = threeaxisrot( -2.*(qin(:,3).*qin(:,4) - qin(:,1).*qin(:,2)), ...
                                    qin(:,1).^2 - qin(:,2).^2 - qin(:,3).^2 + qin(:,4).^2, ...
                                    2.*(qin(:,2).*qin(:,4) + qin(:,1).*qin(:,3)), ...
                                   -2.*(qin(:,2).*qin(:,3) - qin(:,1).*qin(:,4)), ...
                                    qin(:,1).^2 + qin(:,2).^2 - qin(:,3).^2 - qin(:,4).^2);
        
    case 'xyx'
        [r1,r2,r3] = twoaxisrot( 2.*(qin(:,2).*qin(:,3) + qin(:,1).*qin(:,4)), ...
                                -2.*(qin(:,2).*qin(:,4) - qin(:,1).*qin(:,3)), ...
                                 qin(:,1).^2 + qin(:,2).^2 - qin(:,3).^2 - qin(:,4).^2, ...
                                 2.*(qin(:,2).*qin(:,3) - qin(:,1).*qin(:,4)), ...
                                 2.*(qin(:,2).*qin(:,4) + qin(:,1).*qin(:,3)));
        
    case 'xzy'
        [r1,r2,r3] = threeaxisrot( 2.*(qin(:,3).*qin(:,4) + qin(:,1).*qin(:,2)), ...
                                   qin(:,1).^2 - qin(:,2).^2 + qin(:,3).^2 - qin(:,4).^2, ...
                                  -2.*(qin(:,2).*qin(:,3) - qin(:,1).*qin(:,4)), ...
                                   2.*(qin(:,2).*qin(:,4) + qin(:,1).*qin(:,3)), ...
                                   qin(:,1).^2 + qin(:,2).^2 - qin(:,3).^2 - qin(:,4).^2);
        
    case 'xzx'
        [r1,r2,r3] = twoaxisrot( 2.*(qin(:,2).*qin(:,4) - qin(:,1).*qin(:,3)), ...
                                 2.*(qin(:,2).*qin(:,3) + qin(:,1).*qin(:,4)), ...
                                 qin(:,1).^2 + qin(:,2).^2 - qin(:,3).^2 - qin(:,4).^2, ...
                                 2.*(qin(:,2).*qin(:,4) + qin(:,1).*qin(:,3)), ...
                                -2.*(qin(:,2).*qin(:,3) - qin(:,1).*qin(:,4)));
    otherwise
        error(message('aero:quat2angle:unknownRotation', type));
end

    function [r1,r2,r3] = threeaxisrot(r11, r12, r21, r31, r32)
        % find angles for rotations about X, Y, and Z axes
        r1 = atan2( r11, r12 );
        r21(r21 < -1) = -1;
        r21(r21 > 1) = 1;
        r2 = asin( r21 );
        r3 = atan2( r31, r32 );
    end

    function [r1,r2,r3] = twoaxisrot(r11, r12, r21, r31, r32)
        % Check for singularities
        r1comp = and((r11==0),(r12==0));
        r3comp = and((r31==0),(r32==0));
        OrR1R3 = or(r1comp,r3comp);
        NorR1R3 = not(OrR1R3);
        if any(OrR1R3)
            [r1(OrR1R3),r2(OrR1R3),r3(OrR1R3)] = dcm2angle(quat2dcm(qin(OrR1R3,:)),type,'zeror3');      
        end
        if any(NorR1R3)
            r1(NorR1R3) = atan2( r11(NorR1R3), r12(NorR1R3) );
            r21(r21 < -1) = -1;
            r21(r21 > 1) = 1;
            r2(NorR1R3) = acos( r21(NorR1R3) );
            r3(NorR1R3) = atan2( r31(NorR1R3), r32(NorR1R3) );
        end
        r1 = r1';
        r2 = r2';
        r3 = r3';
    end
end

从函数中可以看到，对于我们关注的ZYX的转换方式，它调用threeaxisrot(r11, r12, r21, r31, r32)实现旋转矩阵转欧拉角，结合式(2.1.2)和(4.1)，可知在该函数中：
$r_1 = atan2(2(q_xq_y+q_wq_z), q_w^2+q_x^2-q_y^2-q_z^2) = \theta_z = yaw$
$r_2 =arcsin(-2(q_xq_z-q_wq_y)) = \theta_y = pitch$
$r_3 = atan2(2(q_yq_z+q_wq_x), q_w^2-q_x^2-q_y^2+q_z^2) = \theta_x = roll$
发现Matlab中,将旋转矩阵转换为ZYX顺序的欧拉角的 quat2angle函数与式 $(5.2)$ 是对应的，因此其ZYX顺序的转换结果与ROS中tf::Matrix3x3的getRPY函数得到的欧拉角结果一致。

quat2eul

quat2eul.m 中的 eul = quat2eul( q ) 函数用于将四元数转换成欧拉角。

function eul = quat2eul( q, varargin )
%QUAT2EUL Convert quaternion to Euler angles
%   EUL = QUAT2EUL(QOBJ) converts a quaternion object, QOBJ, into the
%   corresponding Euler angles, EUL. Each quaternion represents 
%   a 3D rotation. QOBJ is an N-element vector of quaternion objects.
%   The output, EUL, is an N-by-3 array of Euler rotation angles with each 
%   row representing one Euler angle set. Rotation angles are in radians.
%
%   EUL = QUAT2EUL(Q) converts a unit quaternion rotation into the corresponding 
%   Euler angles. The input, Q, is an N-by-4 matrix containing N quaternions. 
%   Each quaternion represents a 3D rotation and is of the form q = [w x y z], 
%   with w as the scalar number. Each element of Q must be a real number.
%
%   EUL = QUAT2EUL(___, SEQ) converts unit quaternion into Euler angles.
%   The Euler angles are specified by the body-fixed (intrinsic) axis rotation
%   sequence, SEQ.
%
%   The default rotation sequence is 'ZYX', where the order of rotation
%   angles is Z Axis Rotation, Y Axis Rotation, and X Axis Rotation.
%
%   The following rotation sequences, SEQ, are supported: 'ZYX', 'ZYZ', and
%   'XYZ'.
%
%   Example:
%      % Calculates Euler angles for a quaternion
%      % By default, the ZYX axis order will be used.
%      q = [sqrt(2)/2 0 sqrt(2)/2 0];
%      eul = quat2eul(q)
%
%      % Calculate the Euler angles for a ZYZ rotation
%      qobj = quaternion([0.7071 0.7071 0 0]);
%      eulZYZ = quat2eul(qobj, 'ZYZ')
%
%   See also eul2quat, quaternion

%   Copyright 2014-2018 The MathWorks, Inc.

%#codegen

% Validate the quaternions
q = robotics.internal.validation.validateQuaternion(q, 'quat2eul', 'q');

seq = robotics.internal.validation.validateEulerSequence(varargin{:});

% Normalize the quaternions
q = robotics.internal.normalizeRows(q);

qw = q(:,1);
qx = q(:,2);
qy = q(:,3);
qz = q(:,4);

% Pre-allocate output
eul = zeros(size(q,1), 3, 'like', q);

% The parsed sequence will be in all upper-case letters and validated
switch seq
    case 'ZYX'
        % Cap all inputs to asin to 1, since values >1 produce complex
        % results
        % Since the quaternion is of unit length, this should never happen,
        % but some code generation configuration seem to hit this edge case
        % under some circumstances.
        aSinInput = -2*(qx.*qz-qw.*qy);
        aSinInput(aSinInput > 1) = 1;
        aSinInput(aSinInput < -1) = -1;
        
        eul = [ atan2( 2*(qx.*qy+qw.*qz), qw.^2 + qx.^2 - qy.^2 - qz.^2 ), ...
            asin( aSinInput ), ...
            atan2( 2*(qy.*qz+qw.*qx), qw.^2 - qx.^2 - qy.^2 + qz.^2 )];
        
    case 'ZYZ'
        % Need to convert to intermediate rotation matrix here to avoid
        % singularities
        R = quat2rotm(q);
        eul = rotm2eul(R, 'ZYZ');
        
    case 'XYZ'
        % Prevent singularities as done in ZYX case
        % Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
        % operations
        
        % sin(y) = R13 = 2 * (qx*qz + qy*qw)
        % tan(x) = sin(x) / cos(x) = -R23 / R33
        %        = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2))
        %        = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2)
        % tan(z) = sin(z) / cos(z) = -R12 / R11
        %        = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2))
        %        = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2)
        
        aSinInput = 2*(qx.*qz + qy.*qw);
        aSinInput(aSinInput > 1) = 1;
        aSinInput(aSinInput < -1) = -1;
        
        eul = [ atan2( -2*(qy.*qz - qx.*qw), qw.^2 - qx.^2 - qy.^2 + qz.^2 ), ...
            asin( aSinInput ), ...
            atan2( -2*(qx.*qy - qz.*qw), qw.^2 + qx.^2 - qy.^2 - qz.^2 )];
end

% Check for complex numbers
if ~isreal(eul)
    eul = real(eul);
end

end

在上述quat2eul函数中，对于欧拉角的ZYX顺序，
$\left\{\begin{aligned} &eul [0] = atan2( 2*(qx.*qy+qw.*qz), qw.^2 + qx.^2 - qy.^2 - qz.^2 ) \\ &eul [1] = asin( -2*(qx.*qz-qw.*qy)) \\ &eul[2] = atan2( 2*(qy.*qz+qw.*qx), qw.^2 - qx.^2 - qy.^2 + qz.^2 )];\end{aligned}\right.$

quat2eul的代码实现与式 $(4.2)$ 完全一致。

旋转矩阵 $\to$ 四元数

dcm2quat

dcm2quat.m中的 q = dcm2quat( dcm, varargin ) 函数中，输入为3x3xM的旋转矩阵，输出为Mx4的旋转向量。下面的dcm2quat转换实现与式(2.2)一致，当矩阵的迹 $tr(\mathbf{R}) > 0$ ，按照式(2.2.1)的方式计算四元数；否则，找到 $\mathbf{R}$ 对角元素的最大值所在位置，按照式(2.2.2)计算四元数。

function q = dcm2quat( dcm, varargin )
%  DCM2QUAT Convert direction cosine matrix to quaternion.
%   Q = DCM2QUAT( N ) calculates the quaternion, Q, for given direction
%   cosine matrix, N. N is a 3-by-3-by-M matrix containing M orthogonal
%   direction cosine matrices. N performs the coordinate transformation of
%   a vector in inertial axes to a vector in body axes. Q returns an M-by-4
%   matrix containing M quaternions. Q has its scalar number as the first
%   column.
% 
%   Q = DCM2QUAT( N, ACTION ) uses ACTION for error handling during
%   rotation matrix validation. Specify if an invalid rotation matrix
%   invokes a 'Warning', 'Error', or no action ('None'). The default is
%   'None'.
%
%   Q = DCM2QUAT( N, ACTION, TOL ) uses TOL as a relative error tolerance
%   for rotation matrix validation. It is a scalar value. Rotation matrix
%   validation confirms that the matrix is orthogonal [transpose(N) * N ==
%   1 +/- TOL] and proper [det(N) == 1 +/- TOL]. The default value is
%   eps(2).
%
%   Examples:
%
%   Determine the quaternion from direction cosine matrix:
%      dcm = [0 1 0; 1 0 0; 0 0 -1];
%      q = dcm2quat(dcm)
%
%   Determine the quaternions from multiple direction cosine matrices:
%      dcm        = [ 0 1 0; 1 0 0; 0 0 -1]; 
%      dcm(:,:,2) = [ 0.4330    0.2500   -0.8660; ...
%                     0.1768    0.9186    0.3536; ...
%                     0.8839   -0.3062    0.3536];
%      q = dcm2quat(dcm)
%
%   See also ANGLE2DCM, DCM2ANGLE, QUAT2DCM, QUAT2ANGLE, ANGLE2QUAT.

%   Copyright 2000-2017 The MathWorks, Inc.

narginchk(1, 3);

%Validate inputs
if nargin < 2
    validateDCM(dcm);
elseif nargin < 3
    validateDCM(dcm, varargin{1});
else
    validateDCM(dcm, varargin{1}, varargin{2});
end

for i = size(dcm,3):-1:1

    q(i,4) =  0; 
    
    tr = trace(dcm(:,:,i));

    if (tr > 0)
        sqtrp1 = sqrt( tr + 1.0 );
        
        q(i,1) = 0.5*sqtrp1; 
        q(i,2) = (dcm(2, 3, i) - dcm(3, 2, i))/(2.0*sqtrp1);
        q(i,3) = (dcm(3, 1, i) - dcm(1, 3, i))/(2.0*sqtrp1); 
        q(i,4) = (dcm(1, 2, i) - dcm(2, 1, i))/(2.0*sqtrp1); 
    else
        d = diag(dcm(:,:,i));
        if ((d(2) > d(1)) && (d(2) > d(3)))
            % max value at dcm(2,2,i)
            sqdip1 = sqrt(d(2) - d(1) - d(3) + 1.0 );
            
            q(i,3) = 0.5*sqdip1; 
            
            if ( sqdip1 ~= 0 )
                sqdip1 = 0.5/sqdip1;
            end
            
            q(i,1) = (dcm(3, 1, i) - dcm(1, 3, i))*sqdip1; 
            q(i,2) = (dcm(1, 2, i) + dcm(2, 1, i))*sqdip1; 
            q(i,4) = (dcm(2, 3, i) + dcm(3, 2, i))*sqdip1; 
        elseif (d(3) > d(1))
            % max value at dcm(3,3,i)
            sqdip1 = sqrt(d(3) - d(1) - d(2) + 1.0 );
            
            q(i,4) = 0.5*sqdip1; 
            
            if ( sqdip1 ~= 0 )
                sqdip1 = 0.5/sqdip1;
            end
            
            q(i,1) = (dcm(1, 2, i) - dcm(2, 1, i))*sqdip1;
            q(i,2) = (dcm(3, 1, i) + dcm(1, 3, i))*sqdip1; 
            q(i,3) = (dcm(2, 3, i) + dcm(3, 2, i))*sqdip1; 
        else
            % max value at dcm(1,1,i)
            sqdip1 = sqrt(d(1) - d(2) - d(3) + 1.0 );
            
            q(i,2) = 0.5*sqdip1; 
            
            if ( sqdip1 ~= 0 )
                sqdip1 = 0.5/sqdip1;
            end
            
            q(i,1) = (dcm(2, 3, i) - dcm(3, 2, i))*sqdip1; 
            q(i,3) = (dcm(1, 2, i) + dcm(2, 1, i))*sqdip1; 
            q(i,4) = (dcm(3, 1, i) + dcm(1, 3, i))*sqdip1; 
        end
    end
end

rotm2quat

rotm2quat中的 quat = rotm2quat( R ) 函数，使用了论文：I.Y. Bar-Itzhack, “New method for extracting the quaternion from a rotation matrix,” Journal of Guidance, Control, and Dynamics, vol. 23, no. 6, pp. 1085-1087, 2000 中所使用的新方法来求四元数，和式 $(2.2)$ 不一致，但通过代码测试发现，它转换得到的四元数结果与Eigen和ROS中的方法一致。

function quat = rotm2quat( R )
%ROTM2QUAT Convert rotation matrix to quaternion
%   Q = ROTM2QUAT(R) converts a 3D rotation matrix, R, into the corresponding
%   unit quaternion representation, Q. The input, R, is an 3-by-3-by-N matrix
%   containing N orthonormal rotation matrices.
%   The output, Q, is an N-by-4 matrix containing N quaternions. Each
%   quaternion is of the form q = [w x y z], with w as the scalar number. 
%   Each element of Q must be a real number.
%
%   If the input matrices are not orthonormal, the function will
%   return the quaternions that correspond to the orthonormal matrices
%   closest to the imprecise matrix inputs.
%
%
%   Example:
%      % Convert a rotation matrix to a quaternion
%      R = [0 0 1; 0 1 0; -1 0 0];
%      q = rotm2quat(R)
%
%   References:
%   [1] I.Y. Bar-Itzhack, "New method for extracting the quaternion from a 
%       rotation matrix," Journal of Guidance, Control, and Dynamics, 
%       vol. 23, no. 6, pp. 1085-1087, 2000
%
%   See also quat2rotm

%   Copyright 2014-2018 The MathWorks, Inc.

%#codegen

robotics.internal.validation.validateRotationMatrix(R, 'rotm2quat', 'R');

% Pre-allocate output
quat = zeros(size(R,3), 4, 'like', R);

% Calculate all elements of symmetric K matrix
K11 = R(1,1,:) - R(2,2,:) - R(3,3,:);
K12 = R(1,2,:) + R(2,1,:);
K13 = R(1,3,:) + R(3,1,:);
K14 = R(3,2,:) - R(2,3,:);

K22 = R(2,2,:) - R(1,1,:) - R(3,3,:);
K23 = R(2,3,:) + R(3,2,:);
K24 = R(1,3,:) - R(3,1,:);

K33 = R(3,3,:) - R(1,1,:) - R(2,2,:);
K34 = R(2,1,:) - R(1,2,:);

K44 = R(1,1,:) + R(2,2,:) + R(3,3,:);

% Construct K matrix according to paper
K = [...
    K11,    K12,    K13,    K14;
    K12,    K22,    K23,    K24;
    K13,    K23,    K33,    K34;
    K14,    K24,    K34,    K44];

K = K ./ 3;

% For each input rotation matrix, calculate the corresponding eigenvalues
% and eigenvectors. The eigenvector corresponding to the largest eigenvalue
% is the unit quaternion representing the same rotation.
for i = 1:size(R,3)
    [eigVec,eigVal] = eig(K(:,:,i),'vector');
    [~,maxIdx] = max(real(eigVal));
    quat(i,:) = real([eigVec(4,maxIdx) eigVec(1,maxIdx) eigVec(2,maxIdx) eigVec(3,maxIdx)]);
    
    % By convention, always keep scalar quaternion element positive. 
    % Note that this does not change the rotation that is represented
    % by the unit quaternion, since q and -q denote the same rotation.
    if quat(i,1) < 0
        quat(i,:) = -quat(i,:);
    end
end

end

旋转矩阵 $\to$ 欧拉角

dcm2angle

在dcm2angle.m文件中定义的dcm2angle函数，将旋转矩阵转换为欧拉角，同样关注我们在意的ZYX顺序对应的欧拉角转换，下面截取了该函数的部分实现，很明显发现它注释里的ZYX顺序与式(4.1)不同，下面对应的threeaxisrot操作，也无法和式(4.1)提到的方法对应起来，也就是说，同样的ZYX顺序的旋转矩阵，分别用Matlab，Eigen，ROS提到的方法把它转成欧拉角，得到的欧拉角结果是不同的。

    case 'zyx'
        %     [          cy*cz,          cy*sz,            -sy]
        %     [ sy*sx*cz-sz*cx, sy*sx*sz+cz*cx,          cy*sx]
        %     [ sy*cx*cz+sz*sx, sy*cx*sz-cz*sx,          cy*cx]

        [r1,r2,r3] = threeaxisrot( dcm(1,2,:), dcm(1,1,:), -dcm(1,3,:), ...
                                   dcm(2,3,:), dcm(3,3,:), ...
                                  -dcm(2,1,:), dcm(2,2,:));
                                  
   function [r1,r2,r3] = threeaxisrot(r11, r12, r21, r31, r32, r11a, r12a)
        % find angles for rotations about X, Y, and Z axes
        r1 = atan2( r11, r12 );
        r21(r21 < -1) = -1;
        r21(r21 > 1) = 1;
        r2 = asin( r21 );
        r3 = atan2( r31, r32 );
        if strcmpi( lim, 'zeror3')
            for i = find(abs( r21 ) == 1.0)
                r1(i) = atan2( r11a(i), r12a(i) );
                r3(i) = 0;
            end
        end
    end

欧拉角 $\to$ 旋转矩阵

angle2dcm

angle2dcm.h中angle2dcm函数，将欧拉角转换为旋转矩阵，同样截取感兴趣的ZYX顺序的转换部分：

function dcm = angle2dcm( r1, r2, r3, varargin )

angles = [r1(:) r2(:) r3(:)];
cang = cos( angles );
sang = sin( angles );
case 'zyx'
        %     [          cy*cz,          cy*sz,            -sy]
        %     [ sy*sx*cz-sz*cx, sy*sx*sz+cz*cx,          cy*sx]
        %     [ sy*cx*cz+sz*sx, sy*cx*sz-cz*sx,          cy*cx]

        dcm(1,1,:) = cang(:,2).*cang(:,1);  %% cos(r2)*cos(r1)
        dcm(1,2,:) = cang(:,2).*sang(:,1);
        dcm(1,3,:) = -sang(:,2);
        dcm(2,1,:) = sang(:,3).*sang(:,2).*cang(:,1) - cang(:,3).*sang(:,1);
        dcm(2,2,:) = sang(:,3).*sang(:,2).*sang(:,1) + cang(:,3).*cang(:,1);
        dcm(2,3,:) = sang(:,3).*cang(:,2);
        dcm(3,1,:) = cang(:,3).*sang(:,2).*cang(:,1) + sang(:,3).*sang(:,1);
        dcm(3,2,:) = cang(:,3).*sang(:,2).*sang(:,1) - sang(:,3).*cang(:,1);
        dcm(3,3,:) = cang(:,3).*cang(:,2);

可以看到，上述方式ZYX对应的旋转矩阵形式与上面dcm2angle函数对应的，因此angle2dcm与式(4.2)所写ZYX的旋转矩阵形式不一样；而且，传入的参数顺序r1,r2,r3分别对应的式yaw，pitch，roll。

eul2rotm

eul2rotm.m函数中的 R = eul2rotm( eul, varargin ) 函数，同上截取ZYX的部分代码，可以确定，eul2rotm与式 $(4.1)$ 完全对应，eul2rotm输入参数eul的欧拉角顺序是[yaw, pitch, roll]。

function R = eul2rotm( eul, varargin )

ct = cos(eul);
st = sin(eul);
case 'ZYX'
        %     The rotation matrix R can be constructed as follows by
        %     ct = [cz cy cx] and st = [sy sy sx]
        %
        %     R = [  cy*cz   sy*sx*cz-sz*cx    sy*cx*cz+sz*sx
        %            cy*sz   sy*sx*sz+cz*cx    sy*cx*sz-cz*sx
        %              -sy            cy*sx             cy*cx]
        %       = Rz(tz) * Ry(ty) * Rx(tx)
        
        R(1,1,:) = ct(:,2).*ct(:,1);
        R(1,2,:) = st(:,3).*st(:,2).*ct(:,1) - ct(:,3).*st(:,1);
        R(1,3,:) = ct(:,3).*st(:,2).*ct(:,1) + st(:,3).*st(:,1);
        R(2,1,:) = ct(:,2).*st(:,1);
        R(2,2,:) = st(:,3).*st(:,2).*st(:,1) + ct(:,3).*ct(:,1);
        R(2,3,:) = ct(:,3).*st(:,2).*st(:,1) - st(:,3).*ct(:,1);
        R(3,1,:) = -st(:,2);
        R(3,2,:) = st(:,3).*ct(:,2);
        R(3,3,:) = ct(:,3).*ct(:,2);

欧拉角 $\to$ 四元数

angle2quat

angle2quat.m函数中的angle2quat函数，将欧拉角转成四元数，同理，截取ZYX部分代码：

function q = angle2quat( r1, r2, r3, varargin )
angles = [r1(:) r2(:) r3(:)];

cang = cos( angles/2 );
sang = sin( angles/2 );
case 'zyx'
        q = [ cang(:,1).*cang(:,2).*cang(:,3) + sang(:,1).*sang(:,2).*sang(:,3), ...
            cang(:,1).*cang(:,2).*sang(:,3) - sang(:,1).*sang(:,2).*cang(:,3), ...
            cang(:,1).*sang(:,2).*cang(:,3) + sang(:,1).*cang(:,2).*sang(:,3), ...
            sang(:,1).*cang(:,2).*cang(:,3) - cang(:,1).*sang(:,2).*sang(:,3)];

前面提到，传入的参数顺序r1,r2,r3分别对应的式yaw，pitch，roll，发现这里欧拉角转四元数的实现与式(5.1)是对应的。也就是说同一个欧拉角roll, pitch, yaw 分别用Matlab和ROS的方法来转换成四元数，它们的结果是一样的（Eigen中没有欧拉角直接转四元数的函数实现）。

eul2quat

eul2quat.m 中的 q = eul2quat( eul, varargin ) 函数将欧拉角转成四元数，关注ZYX代码部分，eul2quat与式 $(5.1)$ 完全对应，且eul2rotm输入参数eul的欧拉角顺序是[yaw, pitch, roll]。

function q = eul2quat( eul, varargin )

c = cos(eul/2);
s = sin(eul/2);
case 'ZYX'
        % Construct quaternion
        q = [c(:,1).*c(:,2).*c(:,3)+s(:,1).*s(:,2).*s(:,3), ...
            c(:,1).*c(:,2).*s(:,3)-s(:,1).*s(:,2).*c(:,3), ...
            c(:,1).*s(:,2).*c(:,3)+s(:,1).*c(:,2).*s(:,3), ...
            s(:,1).*c(:,2).*c(:,3)-c(:,1).*s(:,2).*s(:,3)];

小结&代码验证

注意点：

Matlab中，四元数的顺序是：w,x,y,z
Matlab中，欧拉角的对应顺序是[yaw, pitch, roll]

选取代码篇Eigen 中随机生成的欧拉角，来实验两种转换方法（angle2quat, eul2quat; angle2dcm, eul2rotm）间的差异：

close all; clear all; clc

% roll, pitch, yaw:
% -2.51327, 1.0472, 0.722566
% 0.785398, 1.0472, -2.9531

% test1 ---------------------------------------------------------------
euler1 = [ 0.722566, 1.0472, -2.51327]  % yaw,pitch,roll;
quat_angle2quat1 = angle2quat(euler1(1), euler1(2), euler1(3))
quat_eul2quat1 = eul2quat(euler1)
matrix_angle2dcm1 = angle2dcm(euler1(1), euler1(2), euler1(3))
matrix_eul2rotm1 = eul2rotm(euler1)
% [euler1_dcm2angle(1), euler1_dcm2angle(2), euler1_dcm2angle(3)] = dcm2angle(matrix_angle2dcm);
% euler1_dcm2angle
% euler1_rotm2eul = rotm2eul(matrix_eul2rotm1)

% test1 ---------------------------------------------------------------
euler2 = [ -2.9531, 1.0472,0.785398]  % yaw,pitch,roll;
quat_angle2quat2 = angle2quat(euler2(1), euler2(2), euler2(3))
quat_eul2quat2 = eul2quat(euler2)
matrix_angle2dcm2 = angle2dcm(euler2(1), euler2(2), euler2(3))
matrix_eul2rotm2 = eul2rotm(euler2)

运行结果输出如下：

euler1 =
0.7226 1.0472 -2.5133

quat_angle2quat1 =
0.0823 -0.8251 -0.1466 0.5394

quat_eul2quat1 =
0.0823 -0.8251 -0.1466 0.5394

matrix_angle2dcm1 =
0.3751 0.3307 -0.8660
0.1532 -0.9435 -0.2939
-0.9143 -0.0224 -0.4045

matrix_eul2rotm1 =
0.3751 0.1532 -0.9143
0.3307 -0.9435 -0.0224
-0.8660 -0.2939 -0.4045

euler2 =
-2.9531 1.0472 0.7854

quat_angle2quat2 =
-0.1152 0.4911 -0.2865 -0.8146

quat_eul2quat2 =
-0.1152 0.4911 -0.2865 -0.8146

matrix_angle2dcm2 =
-0.4911 -0.0937 -0.8660
-0.4690 -0.8093 0.3536
-0.7340 0.5798 0.3536

matrix_eul2rotm2 =
-0.4911 -0.4690 -0.7340
-0.0937 -0.8093 0.5798
-0.8660 0.3536 0.3536

对比生成结果发现：

eul2rotm方法与ROS中的tf::Matrix3x3的setRPY方法，Eigen中的：eigen_m = Eigen::AngleAxisd(eigen_euler[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(eigen_euler[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(eigen_euler[2], Eigen::Vector3d::UnitX()); 方法，从欧拉角得到的旋转矩阵结果完全一致，而angle2dcm则于ROS和Eigen中的欧拉角转四元数结果不一致。
eul2quat和 angle2quat方法从欧拉角得到的四元数是完全一致的。

下面用Matlab代码来描述旋转顺序及过程：

close all; clear all; clc

%% test1 -----------------------------------------------------
% 创建箭头点集：
x = 0.8 : 0.02 : 1;
y1 = 0.5*(1-x);
y2 = 0.5*(x-1);
z1 = 0.5*(1-x);
z2 = 0.5*(x-1);
xx = 0 : 0.06 : 1;
yy = zeros(size(xx));
zz = zeros(size(xx));
xyz = [xx; yy; zz];
% myshape = xyz;
myshape = cat(2, xyz, [x; y1; z1], [x; y2; z1], [x; y1; z2], [x; y2; z2]);

% 模拟旋转过程，在旋转变换中，欧拉角（roll, pitch, yaw）的含义是：
% 设坐标系原点为O，对于点集ps = [p1, p2, ... , pn], 将原点指向点集中的点的向量 Opi (i=1,2,...,n)
% 先绕z轴转yaw角，再绕y轴转pitch角，再绕x轴转roll角所得到的向量Opi'末端的点，即为旋转后的点pi';
figure('Name', 'test1'); 
plot3(myshape(1,:), myshape(2,:), myshape(3,:));
hold on;
xlabel('x');
ylabel('y');
zlabel('z'); 
for yaw = -pi : pi/8 : pi
    for pitch = -pi/2 : pi/8 : pi/2
        for roll = -pi : pi/5 : pi
            Rz = [cos(yaw), -sin(yaw), 0; sin(yaw), cos(yaw), 0; 0, 0, 1];
            Ry = [cos(pitch), 0, sin(pitch); 0, 1, 0; -sin(pitch), 0, cos(pitch)];
            Rx = [1, 0, 0; 0, cos(roll), -sin(roll); 0, sin(roll), cos(roll)];
            Rzyx = Rz*Ry*Rx;                   % R <==> M
            M = eul2rotm([yaw, pitch, roll]);  % R <==> M
            Q = eul2quat([yaw, pitch, roll]);
            
            myshape_transformed = M*myshape;
            plot3(myshape_transformed(1,:), myshape_transformed(2,:), myshape_transformed(3,:), 'LineWidth', 1);
            axis([-1 1 -1 1 -1 1]);
            pause(0.02);
        end
    end
end 
hold off

%% test2 -----------------------------------------------------
% 创建坐标轴集合，用不同密度的点集来分别表示x,y,z轴：
xxx = 0:0.01:1; x0 = zeros(size(xxx));
yyy = 0:0.05:1; y0 = zeros(size(yyy));
zzz = 0:0.1:1; z0 = zeros(size(zzz));

myshape = cat(2, [xxx; x0; x0], [y0; yyy; y0], [z0; z0; zzz]);
figure('Name', 'test2');
plot3(myshape(1,:), myshape(2,:), myshape(3,:), '*')  % 未旋转
hold on;
yaw = pi/3; pitch = pi/4; roll = 0;
Rz = [cos(yaw), -sin(yaw), 0; sin(yaw), cos(yaw), 0; 0, 0, 1];
Ry = [cos(pitch), 0, sin(pitch); 0, 1, 0; -sin(pitch), 0, cos(pitch)];
Rx = [1, 0, 0; 0, cos(roll), -sin(roll); 0, sin(roll), cos(pitch)];
Rzyx = Rz*Ry*Rx;
M = eul2rotm([yaw, pitch, roll]);
Q = eul2quat([yaw, pitch, roll]);
myshape_transformed = Rzyx * myshape;  % 第一次旋转；（Rzyx 与 M 等价）
v = myshape_transformed(:, end)
plot3(myshape_transformed(1,:), myshape_transformed(2,:), myshape_transformed(3,:), '*','LineWidth', 1);
M = eul2rotm([yaw, pitch, roll+pi/4]);
myshape_transformed = M * myshape;  % 第二次旋转；（Rzyx 与 M 等价）
plot3(myshape_transformed(1,:), myshape_transformed(2,:), myshape_transformed(3,:), '*', 'LineWidth', 1);
axis([-1 1 -1 1 -1 1]);

在下面显示的最终结果中，左图是描述yaw从 $[- p i, p i]$ , pitch从 $[- p i / 2, p i / 2]$ , roll从 $[- p i, p i]$ 旋转结果的全过程；右图是坐标轴做两次旋转后的结果，蓝色，橙色，黄色分别为未旋转，第一次旋转，第二次旋转后的结果。模拟旋转过程，在旋转变换中，欧拉角（roll, pitch, yaw）的含义是：设坐标系原点为O，对于点集 $\mathbf{P} = [P_1, P_2, ... , P_n]$ , 将原点指向点集中的点的向量 $OP_i (i=1,2,...,n)$ 先绕z轴转yaw角，再绕y轴转pitch角，再绕x轴转roll角所得到的向量 $OP_i'$ 末端的点，即为旋转后的点 $P_i'$ 。
在这里插入图片描述

参考

https://blog.youkuaiyun.com/sunqin_csdn/article/details/107969585
https://blog.youkuaiyun.com/sunqin_csdn/article/details/108045463
https://blog.youkuaiyun.com/sunqin_csdn/article/details/108134138

3 条评论

大魔法师_lh 2020.12.06
方便问一下楼主，那个validateDCM函数是用来做什么的，我的matlab不知道发什么疯，使用dcm2angle时报错“函数或变量 'validateDCM' 无法识别。“
- sunqin_csdn回复大魔法师_lh 2020.12.07
  dcm是指方向余弦阵, 看函数名, 它的作用应该是用来判断你输入的旋转矩阵是否合法. 你检查一下你输入的矩阵看看是否满足旋转矩阵的性质. 按理说它里面既然写了这个函数, 这个函数应该是存在的才对, (不然的话或许有可能是MATLAB版本的问题...)
- sunqin_csdn回复大魔法师_lh 2020.12.07
  dcm是指方向余弦阵, 看函数名, 它的作用应该是用来判断你输入的旋转矩阵是否合法, 你可以检查一下你的输入矩阵, 是否满足行列式为1, RR^T = I 等性质, 检查一下它是否合法.