【学习SLAM】Matlab ——旋转矩阵，旋转向量,四元数，欧拉角之间的转换

基于Matlab现有函数下的内容
Matlab ——旋转矩阵，旋转向量,四元数，欧拉角之间的转换
旋转矩阵     dcm     R
四元数     quat     q = [q0 q1 q2 q3]
欧拉角     angle     [row,pitch,yaw]/[r1,r2,r3]

注：以上表格是为了帮助理解的表示

roll(横滚) --X     pitch（俯仰）--Y    yaw（偏航/航向）-- Z

- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 转四元数

旋转矩阵转四元数

q =dcm2quat(R)；

 

欧拉角转四元数

q=angle2quat(r1,r2,r3，S)；

- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
转欧拉角

旋转矩阵转欧拉角

 [r2,r2,r3]=dcm2angle(R, S)

注：得到的结果为弧度，若需要角度需进一步转化

四元数转欧拉角

[r1,r2,r3]=quat2angle([q0 q1 q2 q3]，S)

 

注：S 的选择有12种，【'ZYX','ZYZ’,‘ZXY’,‘ZXZ’,‘YXZ’,‘YXY’,‘YZX’,‘YZY’,‘XYZ’,‘XYX’,‘XZY’,‘XZX’】

       S  默认 ‘ZYX'

- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
转旋转矩阵

四元数转旋转矩阵

R=quat2dcm([q0 q1 q2 q3])

欧拉角转旋转矩阵

R=angle2dcm(r1,r2,r3,S);

R=angle2dcm(yaw/180*pi,pitch/180*pi,roll/180*pi)

注：根据欧拉角是弧度/角度，选择以上操作

- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

若已知旋转矩阵R，求四元数[q0 q1 q2 q2]

则对应的四元数为：

- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

旋转矩阵及旋转向量相互转化 Rodrigues矩阵及matlab实现

处理三维旋转问题时，通常采用旋转矩阵的方式来描述。一个向量乘以旋转矩阵等价于向量以某种方式进行旋转。除了采用旋转矩阵描述外，还可以用旋转向量来描述旋转，旋转向量的长度（模）表示绕轴逆时针旋转的角度（弧度）。旋转向量与旋转矩阵可以通过罗德里格斯（Rodrigues）变换进行转换。
     算法过程如下：

式中，norm为求向量的模。反变换也可以很容易的通过如下公式实现：

matlab实现

% om=[0.02876123785972595215 -1.668149590492248535 -0.0360212959349155426]';
om=[ 0.0009597472380846738815 0.01455024257302284241 0.010445447638630867]';
theta=norm(om);
om=om./theta;
I=eye(3);
rom=[0 -om(3) om(2);
     om(3)  0  -om(1);
     -om(2) om(1) 0];
R = cos(theta)*I+(1-cos(theta))*om*om'+ sin(theta)*rom

q =dcm2quat(R)

 

matlab里有rotationVectorToMatrix和rotationMatrixToVector函数

以上为例

>> R=rotationVectorToMatrix(om)

R =

    0.9998    0.0105   -0.0145
   -0.0104    0.9999    0.0010
    0.0146   -0.0009    0.9999

 rotationVector = rotationMatrixToVector(R)

rotationVector =   -0.0010   -0.0146   -0.0104

---------------------------------------------

 

欧拉角与旋转矩阵转换 matlab 函数 angle2dcm dcm2angle 方向余弦矩阵 机器人坐标变换

在工业机器人坐标变换中，旋转矩阵与欧拉角有几个概念比较容易混淆，根据自己的理解整理如下，如有问题，欢迎指正

因为欧拉角相对旋转矩阵更为直观，直接用绕坐标轴的旋转可以得到，因此将旋转矩阵转换为欧拉角会方便分析

在位姿变换相关资料中（可参照 J. J. Craig 《Introduction to Robotics》），位姿矩阵的描述如下

上面这样的位姿描述，指的是主动旋转矩阵，即向量或子坐标系绕坐标轴主动旋转的旋转矩阵，也是机器人坐标变换通常会采取的旋转方式，写成 matlab 函数如下（a输入‘X’,'Y','Z'，B输入旋转角度）

    function y = Rot3( a ,B )
    b = deg2rad(B);
    switch a
        case 'X'
            y=[1,0,0;0,cos(b),-sin(b);0,sin(b),cos(b)];
        case 'Y'
            y=[cos(b),0,sin(b);0,1,0;-sin(b),0,cos(b)];
        case 'Z'
            y=[cos(b),-sin(b),0;sin(b),cos(b),0;0,0,1];
    end

与此区分的是被动旋转矩阵，向量或子坐标系不变，基坐标系旋转的旋转矩阵，这类被动旋转矩阵与上面的三个矩阵互为转置（互为逆矩阵，对于旋转矩阵是等价的），也可以理解为被动旋转矩阵让向量或子坐标系旋转到基坐标系位置，而 matlab 中的 dcm2angle，angle2dcm 均对应的是被动旋转：

    Mat_t = [cos(pi/3),-sin(pi/3),0;sin(pi/3),cos(pi/3),0;0,0,1];
    [ra,rb,rc]=dcm2angle(Mat_t,'ZYX');
    %此处 ra 为 -60 度，可见 Matlab 中采用的是被动旋转，sin的符号和主动旋转矩阵相反

因此，在采用matlab 中 dcm2angle 计算欧拉角时，需要注意对旋转矩阵进行转置（R3'），以‘ZYX’欧拉角为例：

    R3 = Rot3('Z',20)*Rot3('Y',60)*Rot3('X',40)
    [ra,rb,rc]=dcm2angle(R3','ZYX');
    M3 = Rot3r('Z',ra)*Rot3r('Y',rb)*Rot3r('X',rc)
    d=[ra,rb,rc]./pi.*180

这样，计算结果  d = 20.0000   60.0000   40.0000  ，与常用主动旋转矩阵一致，R3与M3也一致，如果R3‘转置去掉，改为

[ra,rb,rc]=dcm2angle(R3,'ZYX');

则结果为从R3转换到基坐标的欧拉角，对应M3为R3的逆矩阵

需要注意的是，主动/被动旋转不等同于基坐标系、连体坐标系的变换，在主动旋转矩阵下，仍然要区分按照固定基坐标系坐标轴，还是按照旋转后的连体坐标新坐标轴旋转，按照右乘连体左乘基的方式进行计算
————————————————

 

 

旋转向量，旋转矩阵和四元数的关系

参考资料
1.向量旋转公式
2.旋转矩阵和旋转向量
2.1旋转矩阵微分和旋转向量的推导
2.2罗德里格旋转公式
3.四元数和旋转向量
3.1 由欧拉参数推导四元数
3.2旋转向量表示四元数
4.旋转矩阵和四元数
5.欧拉角和旋转向量角

 

参考资料

• Quaternion kinematics for the error-state KF
• barfoot《state estimation forrobotics》
• 袁信、郑锷《捷联式惯性导航原理》
• 以上书籍的下载链接链接：http://pan.baidu.com/s/1c1G0k5U 密码：jdsz

---

刚体在空间中的一次旋转可以用旋转矩阵，四元数和旋转向量三种方式表示，以下总结三者的数学转化关系。

---

1.向量旋转公式

• 旋转向量的定义：方向是旋转轴，大小是旋转角的向量，表示刚体在空间中的一次旋转。
• 定义向量x绕单位旋转轴u旋转角度

• ，将向量绕u轴和垂直u轴分解，并利用向量的点乘的几何意义得到：

  

• 平行于u轴的分量在旋转中保持不变，垂直分量则旋转

• 角度，公式(48)到（49）的推导利用了正交基的几何关系和上式证明的模长相等。

  

2.旋转矩阵和旋转向量

2.1旋转矩阵微分和旋转向量的推导

• 相对于笔记1，利用哥氏定理推导旋转矩阵的微分方程，这里给出了另外一种微分方程的推导，并从旋转角时间积分的角度给出一种旋转向量的定义。

  

2.2罗德里格旋转公式

• 利用李群SO(3)推导出以旋转向量表示的罗德里格旋转公式(Rodrigues rotation formula)；其中计算时旋转轴u用哪个坐标系表示都是等价的，后面将证明。

  

• 利用罗德里格旋转公式推导向量旋转公式

  

3.四元数和旋转向量

3.1 由欧拉参数推导四元数

• 由旋转矩阵的欧拉参数(Euler Parameters)推导单位四元数的表示,这里参考barfoot书上的内容，这部分也证明了罗德里格旋转公式的旋转轴u用哪个坐标系表示都可以。1中也有说明，旋转轴在旋转过程中没有几何位置变化。

  
  

• 由此得到以欧拉参数推导出的单位四元数

  

3.2旋转向量表示四元数

• 根据3.1，由旋转向量表示四元数可以写成：

  

• 用四元数表示向量的旋转，这里和四元数相乘运算的三维向量都改写成四元数的形式（只有虚部，实部为0），并进行了证明(结果等于向量旋转公式结果)：

  

4.旋转矩阵和四元数

• 由2.2和3.3，以向量旋转公式为纽带，得到旋转矩阵和四元数表示旋转的等价关系,进而得到单位四元数的元素表示的旋转矩阵,该结果和3.1由欧拉参数表示罗德里格公式的结论一致。3.1中的欧拉参数其实就是这里的单位四元数的实部和虚部。
• 其中这里的x是以四元数的形式表示的。

• 
  
  
  

5.欧拉角和旋转向量角

• 我们提到欧拉角，一般指的是绝对姿态的欧拉角，即导航系下的机体俯仰滚转和偏航角，而旋转向量角指的是一次旋转过程的角度，一般是微小量。
• 两者的关系：机体在运动过程中欧拉角的更新可以看做是初始旋转矩阵(欧拉角表示)与每次旋转矩阵（旋转向量角表示）的积分。

欧拉角的转动顺序为Z→Y→X，转动的角度分别为(ψ，θ，ϕ)。

欧拉角介绍

         欧拉角包括偏航角(yaw)ψ，俯仰角(pitch)θ，滚转角(roll)ϕ，导航坐标系S−Oxgygzg经过三次旋转可以得到机体坐标系B−xbybzb。需要注意的是，这里旋转的是坐标轴，不是向量。旋转过程遵循右手定则。

• RPY角与Z-Y-X欧拉角

　　描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。第一种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转γ

，然后绕{A}的Y轴旋转β，最后绕{A}的Z轴旋转α

，就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。

　　Roll:横滚

　　Pitch: 俯仰

Yaw: 偏航（航向）