旋转矩阵、欧拉角、四元数、轴/角之间的转换

转自：http://blog.csdn.net/xuehuafeiwu123/article/details/74942989

并做修改

参考：
http://blog.csdn.net/yc461515457/article/details/51126352 http://www.xieguofang.cn/Maths/3D_Rotation/Guofang_Xie%27s_Formulas_for_3D_Rotation_Ch.htm https://en.wikipedia.org/wiki/Rotation_matrix https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
在机器人学中，表示旋转的有四种方式。四种方式之间的转换整理出来如下。

旋转矩阵

欧拉角（RPY）

绕

Z

轴旋转称为回转（Roll），绕

Y

轴旋转称为俯仰（Pitch），绕

X

轴旋转称为偏转（Yaw）。

{A}为参考坐标系，将{A}分别按顺序沿xA,yA,zA旋转γ,β,α后，和{B}重合，{A}和{B}之间的旋转方程：

ABRxyz=(γ,β,α)=R(zA,α)R(yA,β)R(xA,α)

四元数

是角/轴的扩展。

轴/角

描述一个坐标系沿某一条直线旋转一定的角度，即与另一个坐标系重合。

经常要用到他们之间的相互转换。

一、旋转矩阵

1、旋转矩阵转换为欧拉角

ABRxyz(γ,β,α)=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥

⎧⎩⎨⎪⎪⎪⎪β=atan2(−r31,r211+r221−−−−−−−√)∈[−π/2,π/2]α=atan2(r21,r11)γ=atan2(r32,r33)

⎧⎩⎨⎪⎪⎪⎪β=atan2(−r31,r211+r221−−−−−−−√)∈[π/2,3π/2]α=atan2(−r21,−r11)γ=atan2(−r32,−r33)

2、旋转矩阵转化为 角/轴

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥

θ=acos(r11+r22+r33−12)

r→=12sinθ⎡⎣⎢r32−r23r13−r31r21−r12⎤⎦⎥

3、旋转矩阵转化为四元数

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥

w=r11+r22+r33+12

v→=12⎡⎣⎢⎢sgn(r32−r23)r11−r22−r33+1−−−−−−−−−−−−−−√sgn(r13−r31)r22−r11−r33+1−−−−−−−−−−−−−−√sgn(r21−r12)r33−r22−r11+1−−−−−−−−−−−−−−√⎤⎦⎥⎥

二、欧拉角（RPY）

1、欧拉角转换为旋转矩阵

ABRxyz(γ,β,α)=⎡⎣⎢cαcβsαcβ<
1c21a
span class="mtd" id="MathJax-Span-828">−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎤⎦⎥

三、四元数

1、四元数转化为旋转矩阵

R=⎡⎣⎢⎢2(w2+v2x)−12(vxvy+wvz)2(vxvz−wvx)2(vxvy−wvz)2(w2+v2y)−12(vyvz+wvx)2(vxvz+wvx)2(vyvz−wvx)2(w2+v2z)−1⎤⎦⎥⎥

四、轴/角

1、轴/角 转化为旋转矩阵

R=⎡⎣⎢r2x(1−cθ)+cθrxry(1−cθ)+rzsθrxrz(1−cθ)-rysθrxry(1−cθ)−rzsθr2y(1−cθ)+cθryrz(1−cθ)+rxsθrxrz(1−cθ)+rysθryrz(1−cθ)−rxsθr2z(1−cθ)+cθ⎤⎦⎥