旋轉矩陣、尤拉角、四元數、軸/角之間的轉換

ersaijun發表於2017-07-13
矩陣

轉自：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β−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θ⎤⎦⎥

相關文章