如何将四元数方向转化为旋转举证

旋转矩阵：

旋转矩阵转欧拉角

欧拉角：

欧拉角转旋转矩阵

function rot = rpy2rot(roll,pitch,yaw)Cphi = cos(roll);  Sphi = sin(roll);
Cthe = cos(pitch); Sthe = sin(pitch);
Cpsi = cos(yaw);   Spsi = sin(yaw);rot = [Cpsi*Cthe  -Spsi*Cphi+Cpsi*Sthe*Sphi  Spsi*Sphi+Cpsi*Sthe*Cphi;Spsi*Cthe  Cpsi*Cphi+Spsi*Sthe*Sphi   -Cpsi*Sphi+Spsi*Sthe*Cphi;-Sthe      Cthe*Sphi                  Cthe*Cphi                 ];

欧拉角转四元数

%将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转α,β,γ角）转换为四元数：function q=EtoQ(alpha,belta,gamma)
q=[cos(alpha/2)*cos(belta/2)*cos(gamma/2)+sin(alpha/2)*sin(belta/2)*sin(gamma/2);sin(alpha/2)*cos(belta/2)*cos(gamma/2)-cos(alpha/2)*sin(belta/2)*sin(gamma/2);cos(alpha/2)*sin(belta/2)*cos(gamma/2)+sin(alpha/2)*cos(belta/2)*sin(gamma/2);cos(alpha/2)*cos(belta/2)*sin(gamma/2)-sin(alpha/2)*sin(belta/2)*cos(gamma/2);
];
end%[InternetShortcut]
%URL=https://www.cnblogs.com/21207-iHome/p/6894128.html

单位四元数：

单位四元数转旋转矩阵：

 function R = quatern2rotMat(q)[rows,cols] = size(q);R = zeros(3,3, rows);R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4));R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3));R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2;R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2));R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;
end
% ---------------------
% 作者：声时刻
% 来源：CSDN
% 原文：https://blog.csdn.net/shenshikexmu/article/details/53608224?utm_source=copy
% 版权声明：本文为博主原创文章，转载请附上博文链接！

因为

，所以

function R=Quaternion_To_RotationMatrix(q)
s = q(1);
a = q(2);
b = q(3);
c = q(4);
R = [1-2*(b^2+c^2) 2*a*b-2*s*c 2*s*b+2*a*c;2*a*b+2*s*c 1-2*(a^2+c^2) -2*s*a+2*b*c;-2*s*b+2*a*c 2*s*a+2*b*c 1-2*(a^2+b^2)];
end

两种方法是一致的。

function R=quatoR(q)
w=q(1);
x=q(2);
y=q(3);
z=q(4);R=[1-2*y^2-2*z^2 2*x*y+2*w*z 2*x*z-2*w*y;2*x*y-2*w*z 1-2*x^2-2*z^2 2*y*z+2*w*x2*x*z+2*w*y 2*y*z-2*w*x 1-2*x^2-2*y^2];

Matlab自带单位四元数转旋转矩阵：

% function R=QtoR(q)
%
% %q1=quatnormalize(q); %单位化
% R=quat2dcm(q);%q1的第一位是实部q=[cos(pi/6) sin(pi/6)*(1/sqrt(14)) sin(pi/6)*(2/sqrt(14)) sin(pi/6)*(3/sqrt(14))]';
q1=quatnormalize(q); %单位化
R=quat2dcm(q1);%q1的第一位是实部

四元数转欧拉角：

function E=QtoE(q)
w=q(1);
x=q(2);
y=q(3);
z=q(4);Euler_x=atan(2*((w*x)+(y*z)))/(1-2*((x*x)+(y*y)));
Euler_y=asin(2*((w*y)-(x*z)));
Euler_z=atan(2*((w*z)+(x*y))/(1-2*((y*y)+(z*z))));E=[Euler_x;Euler_y;Euler_z];end
%https://www.cnblogs.com/21207-iHome/p/6894128.html

描述三维空间中两个不同笛卡尔坐标系之间姿态（不含位置）关系的方法（求真）：

旋转矩阵
Rodrigues公式（一个旋转轴+一个角度）
欧拉角
四元数

旋转的正方向：

使用右手坐标系统的是用右手螺旋定则定义的.
（右手拇指指向旋转轴的正方向，其它四个手指弯曲的方向即为旋转的方向）

旋转矩阵：

矩阵就是变换，旋转矩阵就是用来做旋转操作的变换。
旋转矩阵是用来描述三维空间中两个不同笛卡尔坐标系之间姿态（不含位置）关系的一个3×3矩阵。

旋转矩阵一般分 3 个理解，

，分别对应绕

轴旋转

角。先直接（可直接记住）写出来如下：

由罗德里格斯（Rodrigues）formula可以推出来这个结果

，代入

即可，以下同理。

这是map（exp）的形式，由一个旋转轴+一个角度

onto映射到

.

什么是

：

一个梨子：

A frame和B frame一开始重合，

B frame 先绕z-axis of B frame 转

角度

然后绕y-axis of B frame(New)转

角度

最后绕z-axis of B frame(New)转

角度

被称为 ZYZ Euler angles，因为分别绕了

旋转。
：B frame 到 A frame 的旋转变换

故该旋转矩阵：

TBD

旋转矩阵与四元数

给定一个单位四元数

，一个旋转矩阵

，那么，对点

的旋转可以表示为：

四元数与3D旋转的关系并不是一对一的，同一个3D旋转可以使用两个不同的四元数来表示．对任意的单位四元数，

和

代表的是同一个旋转．如果

表示的是沿着旋转轴

旋转

度，那么

代表的是沿着相反的旋转轴

旋转

度。

四元数转化为旋转矩阵的公式为：

旋转矩阵转四元数：link

旋转矩阵转欧拉角：看起来很全

二维旋转：(TBD)

现在看看复数与二维旋转的关系：

三维旋转：

思路：旋转的分解

其中

为其大小，

为其方向

因为

而

与旋转轴

正交，由俯视角，可以看作是平面的旋转。

左图中，

为

绕

旋转

右图中，

，我们现在可以把

投影到

和

上，

以下补充推导向量点乘公式（求角度余弦）：

proof:

设向量

为二维向量，

为

与

轴的夹角，

设

为

与

轴的夹角，向量

间的夹角

。

定理

：

（平行情况）

当

平行于旋转轴

时，旋转

角度后的

用四元数可以写为：

2. (垂直情况)

当

正交于旋转轴

时，旋转

角度后的

用四元数可以写为：

欧拉角：

欧拉角转四元数时的单位是什么？取值范围是什么？

图示，旋转步骤如下：

物体绕全局的

轴旋转

角（因为要和世界保持联系，否则就和世界完全没有关系了。）
继续绕自己的
轴旋转

角
最后绕自己的
轴旋转

角

很显然，按照不同的旋转步骤，旋转的结果是不一样的。

会导致

，而且依赖于三个坐标轴的选定。

局部坐标是很直观，但是导致欧拉角有一个重大缺陷，万向节死锁！

要规避万向节死锁，需要选择合适的旋转顺序（有12种旋转顺序），在编程中很难规避死锁问题，所以现在很多时候都使用四元数实现旋转。

欧拉角是用来表示三维坐标系中方向和方向变换的。要指出，我们平时说的欧拉角其实还可以细分为欧拉角(Euler-angles)和泰特布莱恩角(Tait-Bryan-angles)，这两种方法都利用了笛卡尔坐标系的三轴作为旋转轴，主要区别在于选取顺序。欧拉角的选取顺序有

这6种，可见选取顺序是a,b,a这样的顺序，也就是

绕a轴旋转某角度后，绕新生成的b轴旋转一个角度，最后绕两次旋转以后的a轴再旋转一个角度，以此表示最终的方向。泰特布莱恩角的旋转轴选取有

这6种，也就是

历遍笛卡尔坐标系的三轴，比如我们最常见到的Roll-Pitch-Yaw角就是其中

的情况。但这两种方法，其实都是在空间中用最直观的方式和最少的参数表示任意方向的通用方法，用它们表示方向没有计算要求和容量需求的区别。

——出处

如何通俗地解释欧拉角？之后为何要引入四元数？www.zhihu.com

四元数：

四元数

是由爱尔兰数学家哈密顿

在

年提出。大多数人用汉密尔顿四元数就只是做三维空间的旋转变换。

三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的

（轴角式）表示方法。这种表示方法里，Axis可用一个三维向量

来表示，θ可以用一个角度值来表示，直观来讲，一个四维向量

就可以表示出三维空间任意的旋转。

右手坐标系统，也就是说旋转的正方向是用右手螺旋定则定义的．你可以用右手拇指指向旋转轴

的正方向，其它四个手指弯曲的方向即为旋转的方向．

如果∥q∥ = 1，也就是说q是一个单位四元数(UnitQuaternion)，单位四元数（norm=1）则存在于四维空间的一个球面上。因为这个映射是满射，我们可以说所有的单位四元数都对应着一个3D旋转．或者说，一个四维单位超球面（也叫做

）上任意一点所对应的四元数（∥q∥ = 1）都对应着一个3D旋转，这一点会在之后讨论旋转插值的时候会用到。

表格最左列中一个元素右乘以顶行中一个元素的结果就位于这两个元素行列的交叉处

数值计算怎么防止矩阵出现数值问题

四元数插值：

先复习一下怎么用四元数旋转矢量

,可对照下图

只需要三步：

第一步，定义纯四元数

第二步，确定旋转轴

,比如

，转过的角度

那么对应的变换四元数

此时它的共轭

对于单位四元数来说，它的逆

第三步，进行四元数运算，则在这个旋转下的新的坐标

其中，

，即对四元数

进行一个

的左旋转，和一个

的右旋转。

四元数左乘

右乘。

那么，依据上图，假设设有两个旋转变换

和

，我们希望找出一些中间变换

，让初始变换

能够平滑地过渡到最终变换

．

的取值可以是

．当

时，

等同于初始变换

，而

时，

等同于最终变换

．

由于插值的对象是两个变换，想象起来可能非常困难，我们不妨假设3D空间中有任意一个向量

．那么变换四元数

会将

变换到四元数形式的

，而变换四元数

会将

变换到四元数形式的

．我们需要找出中间向量的四元数形式

所对应的变换

，使

旋转到

与

中间的某个位置

。

我们可以看到，这个旋转的变化量其实对应的仍是一个旋转．它将由

变换到

的向量进一步旋转到

．这个旋转拥有某一个固定的旋转轴

，我们只需要缩放这个变换所对应的⻆度

就能够达到插值的目的了．

page38

四元数乘法的顺序，和矩阵与函数的复合非常相似，都是从右往左叠加．