坐标变换学习笔记—代码篇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)$ 来求旋转轴，即：
$$v=\frac{[4q_0{q}_{1}4q_0{q}_{2}4q_0{q}_3{]}^T}{4q_{0}sin(\frac{\theta }{2})}=\frac{[4q_0{q}_{1}4q_0{q}_{2}4q_0{q}_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表示对应的欧拉角的旋转顺序，它的值为：${}^{\prime }ZY{X}^{\prime }{,}^{\prime }ZY{Z}^{\prime }{,}^{\prime }ZX{Y}^{\prime }{,}^{\prime }ZX{Z}^{\prime }{,}^{\prime }YX{Z}^{\prime }{,}^{\prime }YX{Y}^{\prime }{,}^{\prime }YZ{X}^{\prime }{,}^{\prime }YZ{Y}^{\prime }{,}^{\prime }XY{Z}^{\prime }{,}^{\prime }XY{X}^{\prime }{,}^{\prime }XZ{Y}^{\prime }{,}^{\prime }XZ{X}^{\prime }.$ 中的一个（默认值为：ZYX），代表依次按照该轴的顺序旋转。（目前对为什么有ZYZ, YZY, XYX,…, 这种某个轴转两次的情况不太清楚（欢迎知道原因的大佬解答），存疑，后面知道再补充）。
对于顺序：${}^{\prime }ZY{X}^{\prime }{,}^{\prime }ZX{Y}^{\prime }{,}^{\prime }YX{Z}^{\prime }{,}^{\prime }YZ{X}^{\prime }{,}^{\prime }XY{Z}^{\prime }{,}^{\prime }XZ{Y}^{\prime }$，r1,r3角度范围为$-\pi \sim \pi$； 而r2的范围为： − π 2 ∼ π 2 -\frac{\pi}{2} \thicksim \frac{\pi}{2} −2π​∼