Bootstrap

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

在机器人学中,表示旋转的有四种方式。不同的人可能习惯于用不同的方法,现将四种方式之间的转换整理出来如下。

  • 旋转矩阵
旋转矩阵R表示坐标系`O-x'y'z'`中的向量坐标变换为同一向量在坐标系`O-xyz`中的坐标的变换矩阵(transformation matrix)。
p=Rp' 
p'=R'p

旋转矩阵属于特殊正交群(special orthonormal group);正交矩阵,每一列为单位矩阵,行列式为1。

- 描述了两个坐标系之间的相对指向。

- 表示了同一点在不同坐标系下(原点相同,即只有转动,没用平动)的坐标之间的坐标变换。

- 是将向量在同一坐标系下进行旋转的算子。	
  • 欧拉角(RPY)

    Z轴旋转称为回转(Roll),绕Y轴旋转称为俯仰(Pitch),绕X轴旋转称为偏转(Yaw)。

    {A}为参考坐标系,将{A}分别按顺序沿 x A , y A , z A x_A,y_A,z_A xA,yA,zA旋转 γ , β , α \gamma, \beta, \alpha γ,β,α后,和{B}重合,{A}和{B}之间的旋转方程:
    B A R x y z = ( γ , β , α ) = R ( z A , α ) R ( y A , β ) R ( x A , α ) _B^A R_{xyz} = (\gamma, \beta, \alpha) = R(z_A, \alpha)R(y_A, \beta)R(x_A,\alpha ) BARxyz=(γ,β,α)=R(zA,α)R(yA,β)R(xA,α)

  • 四元数

      是角/轴的扩展。
    
  • 轴/角

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

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

##一、旋转矩阵

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

B A R x y z ( γ , β , α ) = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] _B^A R_{xyz} (\gamma, \beta, \alpha)= \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} BARxyz(γ,β,α)=r11r21r31r12r22r32r13r23r33

{ β = a t a n 2 ( − r 31 , r 11 2 + r 21 2 ) α = a t a n 2 ( r 21 , r 11 ) , i f β ∈ [ − π / 2 , π / 2 ] γ = a t a n 2 ( r 32 , r 33 ) , i f β ∈ [ − π / 2 , π / 2 ] \begin{cases} \beta = atan2(-r_{31}, \sqrt{r_{11}^2 + r_{21}^2}) \\ \alpha = atan2(r_{21}, r_{11}), if \beta \in [-\pi/2,\pi/2]\\ \gamma = atan2(r_{32},r_{33}), if \beta \in [-\pi/2,\pi/2] \end{cases} β=atan2(r31,r112+r212 )α=atan2(r21,r11),ifβ[π/2,π/2]γ=atan2(r32,r33),ifβ[π/2,π/2]
或者:
{ β = a t a n 2 ( − r 31 , r 11 2 + r 21 2 ) α = a t a n 2 ( − r 21 , − r 11 ) , i f β ∈ [ π / 2 , 3 π / 2 ] γ = a t a n 2 ( − r 32 , − r 33 ) , i f β ∈ [ π / 2 , 3 π / 2 ] \begin{cases} \beta = atan2(-r_{31}, \sqrt{r_{11}^2 + r_{21}^2}) \\ \alpha = atan2(-r_{21}, -r_{11}), if \beta \in [\pi/2,3\pi/2]\\ \gamma = atan2(-r_{32},-r_{33}), if \beta \in [\pi/2,3\pi/2] \end{cases} β=atan2(r31,r112+r212 )α=atan2(r21,r11),ifβ[π/2,3π/2]γ=atan2(r32,r33),ifβ[π/2,3π/2]

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

R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R= \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} R=r11r21r31r12r22r32r13r23r33

θ = a c o s ( r 11 + r 22 + r 33 − 1 2 ) \theta =acos(\frac{ r_{11} + r_{22} + r_{33} -1}{2}) θ=acos(2r11+r22+r331)
r → = 1 2 s i n θ [ r 32 − r 23 r 13 − r 31 r 21 − r 12 ] \overrightarrow r = \frac{1}{2sin \theta}\begin{bmatrix} r_{32}-r_{23} \\ r_{13}-r_{31} \\ r_{21} - r_{12} \end{bmatrix} r =2sinθ1r32r23r13r31r21r12

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

R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R= \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} R=r11r21r31r12r22r32r13r23r33

w = r 11 + r 22 + r 33 + 1 2 w = \frac {\sqrt { r_{11} + r_{22} + r_{33} +1}}{2} w=2r11+r22+r33+1

v → = 1 2 [ s g n ( r 32 − r 23 ) r 11 − r 22 − r 33 + 1 s g n ( r 13 − r 31 ) r 22 − r 11 − r 33 + 1 s g n ( r 21 − r 12 ) r 33 − r 22 − r 11 + 1 ] \overrightarrow v = \frac{1}{2}\begin{bmatrix} sgn (r_{32}-r_{23}) \sqrt {r_{11} - r_{22} - r_{33} +1} \\ sgn(r_{13}-r_{31}) \sqrt {r_{22} - r_{11} - r_{33} +1}\\ sgn(r_{21}-r_{12}) \sqrt {r_{33} - r_{22} - r_{11} +1} \end{bmatrix} v =21sgn(r32r23)r11r22r33+1 sgn(r13r31)r22r11r33+1 sgn(r21r12)r33r22r11+1

##二、欧拉角(RPY)

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

B A R x y z ( γ , β , α ) = [ c α c β c α s β s γ − s α c γ c α s β c γ − s α s γ s α c β s α s β s γ − c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] _B^A R_{xyz} (\gamma, \beta, \alpha) = \begin{bmatrix} c_\alpha c_ \beta & c_\alpha s_ \beta s_ \gamma - s_ \alpha c_ \gamma & c_\alpha s_ \beta c_\gamma - s_ \alpha s_ \gamma \\ s_\alpha c_ \beta & s_\alpha s_ \beta s_ \gamma - c_ \alpha c_ \gamma & s_\alpha s_ \beta c_\gamma - c _\alpha s_ \gamma \\ -s_ \beta & c_\beta s_\gamma & c_\beta c_\gamma \end{bmatrix} BARxyz(γ,β,α)=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 ( w 2 + v x 2 ) − 1 2 ( v x v y − w v z ) 2 ( v x v z + w v x ) 2 ( v x v y + w v z ) 2 ( w 2 + v y 2 ) − 1 2 ( v y v z − w v x ) 2 ( v x v z − w v x ) 2 ( v y v z + w v x ) 2 ( w 2 + v z 2 ) − 1 ] R= \begin{bmatrix} 2(w^2+v_x^2)-1 & 2(v_xv_y-wv_z) & 2(v_xv_z+wv_x) \\ 2(v_xv_y+wv_z) & 2(w^2+v_y^2)-1 & 2(v_yv_z-wv_x) \\ 2(v_xv_z-wv_x) & 2(v_yv_z+wv_x) & 2(w^2+v_z^2)-1 \end{bmatrix} R=2(w2+vx2)12(vxvy+wvz)2(vxvzwvx)2(vxvywvz)2(w2+vy2)12(vyvz+wvx)2(vxvz+wvx)2(vyvzwvx)2(w2+vz2)1

##四、轴/角

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

R = [ r x 2 ( 1 − c θ ) + c θ r x r y ( 1 − c θ ) − r z s θ r x r z ( 1 − c θ ) + r y s θ r x r y ( 1 − c θ ) + r z s θ r y 2 ( 1 − c θ ) + c θ r y r z ( 1 − c θ ) − r x s θ r x r z ( 1 − c θ ) − r y s θ r y r z ( 1 − c θ ) + r x s θ r z 2 ( 1 − c θ ) + c θ ] R= \begin{bmatrix} r_x^2(1-c\theta) + c\theta & r_xr_y(1-c\theta) -r_zs\theta & r_xr_z(1-c\theta) +r_ys\theta \\ r_xr_y(1-c\theta) +r_zs\theta & r_y^2(1-c\theta) + c\theta & r_yr_z(1-c\theta) -r_xs\theta \\ r_xr_z(1-c\theta) -r_ys\theta & r_yr_z(1-c\theta) +r_xs\theta & r_z^2(1-c\theta) + c\theta \end{bmatrix} R=rx2(1cθ)+cθrxry(1cθ)+rzsθrxrz(1cθ)rysθrxry(1cθ)rzsθry2(1cθ)+cθryrz(1cθ)+rxsθrxrz(1cθ)+rysθryrz(1cθ)rxsθrz2(1cθ)+cθ

;