四元数、欧拉角、旋转矩阵笔记
先定义坐标系。x轴朝向自己，y轴朝右，z轴向上。各角度的正方向均为从外侧看向原点绕各个轴逆时针旋转。
四元数
单位四元数
Q = [ cos ϕ 2 , i sin ϕ 2 , j sin ϕ 2 , k sin ϕ 2 ] \begin{aligned} & Q=\left[\cos\frac\phi 2,i\sin\frac\phi 2,j\sin\frac\phi 2, k\sin\frac\phi 2\right] \\ \end{aligned} Q=[cos2ϕ,isin2ϕ,jsin2ϕ,ksin2ϕ]
四元数性质
q a q b = [ s a , a ⃗ ] [ s b , b ⃗ ] = [ s a s b − a ⃗ ⋅ b ⃗ , s a b ⃗ + s b a ⃗ + a ⃗ × b ⃗ ] q − 1 = q ∗ ∣ q ∣ 2 \begin{aligned} & q_aq_b=[s_a,\vec{a}][s_b,\vec{b}] =[s_as_b-\vec{a}\cdot\vec{b}, s_a\vec{b} + s_b\vec{a} + \vec{a}\times\vec{b}] \\ & q^{-1} = \frac{q^*}{|q|^2} \end{aligned} qaqb=[sa,a][sb,b]=[sasb−a⋅b,sab+sba+a×b]q−1=∣q∣2q∗
任意向量 v ⃗ \vec{v} v 沿着以单位向量定义的旋转轴 u ⃗ \vec{u} u 旋转 θ \theta θ 度之后的 v ⃗ ′ \vec{v}' v′ 可以使用四元数乘法来获得。令 q v = [ 0 , v ⃗ ] q_v=[0,\vec{v}] qv=[0,v]， q = [ cos θ 2 , sin θ 2 u ⃗ ] q=[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\vec{u}] q=[cos2θ,sin2θu]，那么
v ⃗ ′ = q q v q ∗ \vec{v}'=qq_vq^* v′=qqvq∗
令单位四元数
q = [ w , x , y , z ] = [ cos θ 2 , i sin θ 2 , j sin θ 2 , k sin θ 2 ] q=[w,x,y,z]=\left[\cos\frac\theta 2,i\sin\frac\theta 2,j\sin\frac\theta 2, k\sin\frac\theta 2\right] q=[w,x,y,z]=[cos2θ,isin2θ,jsin2θ,ksin2θ]
其中 i , j , k i,j,k i,j,k 是旋转轴单位向量， θ \theta θ 是旋转角。则
x 2 + y 2 + z 2 = i 2 + j 2 + k 2 sin θ 2 = sin θ 2 \sqrt{x^2+y^2+z^2} =\sqrt{i^2+j^2+k^2}\sin\frac\theta 2 = \sin\frac\theta 2 x2+y2+z2=i2+j2+k2sin2θ=sin2θ
可求出 θ \theta θ 和 i , j , k i,j,k i,j,k，即旋转轴 n ⃗ \vec{n} n。
cos θ = 1 − 2 sin 2 θ 2 = 1 − 2 ( x 2 + y 2 + z 2 ) = w 2 − x 2 − y 2 − z 2 sin θ = 2 sin θ 2 cos θ 2 = 2 w x 2 + y 2 + z 2 = 2 w 1 − w 2 \begin{aligned} & \cos\theta=1-2\sin^2\frac\theta 2=1-2(x^2+y^2+z^2) =w^2-x^2-y^2-z^2 \\ & \sin\theta=2\sin\frac\theta 2\cos\frac\theta 2=2w\sqrt{x^2+y^2+z^2} =2w\sqrt{1-w^2} \end{aligned} cosθ=1−2sin22θ=1−2(x2+y2+z2)=w2−x2−y2−z2sinθ=2sin2θcos2θ=2wx2+y2+z2=2w1−w2
旋转矩阵
横滚角 ϕ \phi ϕ 绕x轴旋转，向左横滚为正
R x = [ 1 0 0 0 cos ϕ − sin ϕ 0 sin ϕ cos ϕ ] R_x=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix} Rx= 1000cosϕsinϕ0−sinϕcosϕ
俯仰角 θ \theta θ 绕y轴旋转，抬头为正
R y = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_y=\begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \\ \end{bmatrix} Ry= cosθ0−sinθ010sinθ0cosθ
偏航角 ψ \psi ψ 绕z轴旋转，向左偏航为正
R z = [ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] R_z=\begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz= cosψsinψ0−sinψcosψ0001
由罗德里格斯公式
R = cos θ I + ( 1 − cos θ ) n ⃗ n ⃗ T + sin θ n ⃗ ∧ R=\cos\theta I+(1-\cos\theta)\vec{n}\vec{n}^{\text{T}} +\sin\theta\vec{n}^{\wedge} R=cosθI+(1−cosθ)nnT+sinθn∧
可以将旋转矩阵和轴角互相转换，有了轴角即可转换成四元数。
由旋转矩阵转换成轴角的公式为
θ = ± arccos ( tr ( R ) − 1 2 ) \theta=\pm\arccos(\frac{\text{tr}(\mathbf{R})-1}{2}) θ=±arccos(2tr(R)−1)
由于 n \boldsymbol{n} n为旋转轴且满足
R n = n \mathbf{R}\boldsymbol{n}=\boldsymbol{n} Rn=n
所以 n \boldsymbol{n} n 为旋转矩阵 R \mathbf{R} R 的特征值为1对应的模长为1的特征向量。
欧拉角
统一按照zyx的旋转顺序，先偏航，再俯仰，最后横滚。
四元数转旋转矩阵
Quaternion to Rotation Matrix -OpenGL
q p = ( s + i x + j y + k z ) ( s p + i x p + j y p + k z p ) = s s p + i s x p + j s y p + k s z p + i x s p + i i x x p + i j x y p + i k x z p + j y s p + j i y x p + j j y y p + j k y z p + k z s p + k i z x p + k j z y p + k k z z p = s s p + i s x p + j s y p + k s z p + i x s p − x x p + k x y p − j x z p + j y s p − k y x p − y y p + i y z p + k z s p + j z x p − i z y p − z z p = s s p − x x p − y y p − z z p + i ( s x p + x s p + y z p − z y p ) + j ( s y p − x z p + y s p + z x p ) + k ( s z p + x y p − y x p + z s p ) \begin{aligned} q p= & (s+i x+j y+k z)\left(s_{p}+i x_{p}+j y_{p}+k z_{p}\right) \\ = & s s_{p}+i s x_{p}+j s y_{p}+k s z_{p}+ \\ & i x s_{p}+i i x x_{p}+i j x y_{p}+i k x z_{p}+ \\ & j y s_{p}+j i y x_{p}+j j y y_{p}+j k y z_{p}+ \\ & k z s_{p}+k i z x_{p}+k j z y_{p}+k k z z_{p} \\ = & s s_{p}+i s x_{p}+j s y_{p}+k s z_{p}+ \\ & i x s_{p}-x x_{p}+k x y_{p}-j x z_{p}+ \\ & j y s_{p}-k y x_{p}-y y_{p}+i y z_{p}+ \\ & k z s_{p}+j z x_{p}-i z y_{p}-z z_{p} \\ = & s s_{p}-x x_{p}-y y_{p}-z z_{p}+ \\ & i\left(s x_{p}+x s_{p}+y z_{p}-z y_{p}\right)+ \\ & j\left(s y_{p}-x z_{p}+y s_{p}+z x_{p}\right)+ \\ & k\left(s z_{p}+x y_{p}-y x_{p}+z s_{p}\right) \end{aligned} qp====(s+ix+jy+kz)(sp+ixp+jyp+kzp)ssp+isxp+jsyp+kszp+ixsp+iixxp+ijxyp+ikxzp+jysp+jiyxp+jjyyp+jkyzp+kzsp+kizxp+kjzyp+kkzzpssp+isxp+jsyp+kszp+ixsp−xxp+kxyp−jxzp+jysp−kyxp−yyp+iyzp+kzsp+jzxp−izyp−zzpssp−xxp−yyp−zzp+i(sxp+xsp+yzp−zyp)+j(syp−xzp+ysp+zxp)+k(szp+xyp−yxp+zsp)
q p = ( s + i x + j y + k z ) ( s p + i x p + j y p + k z p ) = s s p − x x p − y y p − z z p + i ( s x p + s p x + y z p − z y p ) + j ( s y p + s p y − x z p + z x p ) + k ( s z p + s p z + x y p − y x p ) \begin{aligned} qp =& (s+ix+jy+kz)(s_p+ix_p+jy_p+kz_p)\\ =& ss_p-xx_p-yy_p-zz_p+\\ & i(sx_p+s_px+yz_p-zy_p)+\\ & j(sy_p+s_py-xz_p+zx_p)+\\ & k(sz_p+s_pz+xy_p-yx_p) \end{aligned} qp==(s+ix+jy+kz)(sp+ixp+jyp+kzp)ssp−xxp−yyp−zzp+i(sxp+spx+yzp−zyp)+j(syp+spy−xzp+zxp)+k(szp+spz+xyp−yxp)
令
S = s s p − x x p − y y p − z z p X = s x p + x s p + y z p − z y p Y = s y p − x z p + y s p + z x p Z = s z p + x y p − y x p + z s p q p = S + i X + j Y + k Z \begin{aligned} \mathbb{S} =& s s_{p}-x x_{p}-y y_{p}-z z_{p} \\ \mathbb{X} =& s x_{p}+x s_{p}+y z_{p}-z y_{p} \\ \mathbb{Y} =& s y_{p}-x z_{p}+y s_{p}+z x_{p} \\ \mathbb{Z} =& s z_{p}+x y_{p}-y x_{p}+z s_{p} \\ qp =& \mathbb{S}+i\mathbb{X}+j\mathbb{Y}+k\mathbb{Z} \end{aligned} S=X=Y=Z=qp=ssp−xxp−yyp−zzpsxp+xsp+yzp−zypsyp−xzp+ysp+zxpszp+xyp−yxp+zspS+iX+jY+kZ
继续计算
q p q ∗ = ( S + i X + j Y + k Z ) ( s − i x − j y − k z ) = S s + X x + Y y + Z z + i ( − S x + X s − Y z + Z y ) + j ( − S y + X z + Y s − Z x ) + k ( − S z − X y + Y x + Z s ) \begin{aligned} q p q^{*}= & (\mathbb{S}+i \mathbb{X}+j \mathbb{Y}+k \mathbb{Z})(s-i x-j y-k z) \\ = & \mathbb{S} s+\mathbb{X} x+\mathbb{Y} y+\mathbb{Z} z+ \\ & i(-\mathbb{S} x+\mathbb{X} s-\mathbb{Y} z+\mathbb{Z} y)+ \\ & j(-\mathbb{S} y+\mathbb{X} z+\mathbb{Y} s-\mathbb{Z} x)+ \\ & k(-\mathbb{S} z-\mathbb{X} y+\mathbb{Y} x+\mathbb{Z} s) \end{aligned} qpq∗==(S+iX+jY+kZ)(s−ix−jy−kz)Ss+Xx+Yy+Zz+i(−Sx+Xs−Yz+Zy)+j(−Sy+Xz+Ys−Zx)+k(−Sz−Xy+Yx+Zs)
旋转矩阵转四元数
Conversion of rotation matrix to quaternion -stackexchange
Converting a Rotation Matrix to a Quaternion -stackexchange
Maths - Conversion Matrix to Quaternion -EuclideanSpace
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 x 2 x z − 2 w y 2 y z + 2 w x 1 − 2 x 2 − 2 y 2 ] = [ w 2 + x 2 − y 2 − z 2 2 x y − 2 w z 2 x z + 2 w y 2 x y + 2 w z w 2 − x 2 + y 2 − z 2 2 y z − 2 w x 2 x z − 2 w y 2 y z + 2 w x w 2 − x 2 − y 2 + z 2 ] = ( w 2 − q v T q v ) I + 2 q v q v T + 2 w q v ∧ = ( w 2 − x 2 − y 2 − z 2 ) I + 2 [ x 2 x y x z x y y 2 y z x z y z z 2 ] + 2 w [ 0 − z y x 0 − x − y z 0 ] \begin{aligned} R =& \begin{bmatrix} 1-2y^2-2z^2 & 2xy-2wz & 2xz+2wy \\ 2xy+2wz & 1-2x^2-2z^2 & 2yz-2wx \\ 2xz-2wy & 2yz+2wx & 1-2x^2-2y^2 \end{bmatrix} \\ =& \begin{bmatrix} w^2+x^2-y^2-z^2 & 2xy-2wz & 2xz+2wy \\ 2xy+2wz & w^2-x^2+y^2-z^2 & 2yz-2wx \\ 2xz-2wy & 2yz+2wx & w^2-x^2-y^2+z^2 \end{bmatrix} \\ =& (w^2-q_v^\text{T}q_v)I+2q_vq_v^\text{T}+2wq_v^{\wedge} \\ =& (w^2-x^2-y^2-z^2)I +2\begin{bmatrix} x^2 & xy & xz \\ xy & y^2 & yz \\ xz & yz & z^2 \\ \end{bmatrix} +2w\begin{bmatrix} 0 & -z & y \\ x & 0 & -x \\ -y & z & 0 \\ \end{bmatrix} \end{aligned} R==== 1−2y2−2z22xy+2wz2xz−2wy2xy−2wz1−2x2−2z22yz+2wx2xz+2wy2yz−2wx1−2x2−2y2 w2+x2−y2−z22xy+2wz2xz−2wy2xy−2wzw2−x2+y2−z22yz+2wx2xz+2wy2yz−2wxw2−x2−y2+z2 (w2−qvTqv)I+2qvqvT+2wqv∧(w2−x2−y2−z2)I+2 x2xyxzxyy2yzxzyzz2 +2w 0x−y−z0zy−x0
有如下关系
1 + r 11 − r 22 − r 33 = 4 x 2 1 − r 11 + r 22 − r 33 = 4 y 2 1 − r 11 − r 22 + r 33 = 4 z 2 r 12 + r 21 = 4 x y r 13 + r 31 = 4 x z r 23 + r 32 = 4 y z r 21 − r 12 = 4 w z r 13 − r 31 = 4 w y r 32 − r 23 = 4 w x 1+r_{11}-r_{22}-r_{33}=4x^2 \\ 1-r_{11}+r_{22}-r_{33}=4y^2 \\ 1-r_{11}-r_{22}+r_{33}=4z^2 \\ r_{12}+r_{21}=4xy \\ r_{13}+r_{31}=4xz \\ r_{23}+r_{32}=4yz \\ r_{21}-r_{12}=4wz \\ r_{13}-r_{31}=4wy \\ r_{32}-r_{23}=4wx \\ 1+r11−r22−r33=4x21−r11+r22−r33=4y21−r11−r22+r33=4z2r12+r21=4xyr13+r31=4xzr23+r32=4yzr21−r12=4wzr13−r31=4wyr32−r23=4wx
这9个方程中，等式右边包含 4 x , 4 y , 4 z , 4 w 4x,4y,4z,4w 4x,4y,4z,4w 项的方程分别有4,4,4,3个，等式两侧同时除以共同项后即可剩下 x , y , z , w x,y,z,w x,y,z,w 项，而共同项 4 x , 4 y , 4 z 4x,4y,4z 4x,4y,4z 的计算方法为
4 x = ± 2 4 x 2 4x=\pm 2\sqrt{4x^2} 4x=±24x2
式(7)(8)(9)除以共同项后剩下 x , y , z x,y,z x,y,z 项，共同项 4 w 4w 4w 的计算方法为，将式(1)(2)(3)相加得到
3 − r 11 − r 22 − r 33 = 4 ( 1 − w 2 ) 4 w 2 = 1 + r 11 + r 22 + r 33 4 w = ± 2 4 w 2 3-r_{11}-r_{22}-r_{33}=4(1-w^2) \\ 4w^2=1+r_{11}+r_{22}+r_{33} \\ 4w=\pm 2\sqrt{4w^2} 3−r11−r22−r33=4(1−w2)4w2=1+r11+r22+r334w=±24w2
欧拉角转旋转矩阵
https://en.wikipedia.org/wiki/Euler_angles
X 1 Z 2 X 3 = [ c 2 − c 3 s 2 s 2 s 3 c 1 s 2 c 1 c 2 c 3 − s 1 s 3 − c 3 s 1 − c 1 c 2 s 3 s 1 s 2 c 1 s 3 + c 2 c 3 s 1 c 1 c 3 − c 2 s 1 s 3 ] X 1 Z 2 Y 3 = [ c 2 c 3 − s 2 c 2 s 3 s 1 s 3 + c 1 c 3 s 2 c 1 c 2 c 1 s 2 s 3 − c 3 s 1 c 3 s 1 s 2 − c 1 s 3 c 2 s 1 c 1 c 3 + s 1 s 2 s 3 ] X 1 Y 2 X 3 = [ c 2 s 2 s 3 c 3 s 2 s 1 s 2 c 1 c 3 − c 2 s 1 s 3 − c 1 s 3 − c 2 c 3 s 1 − c 1 s 2 c 3 s 1 + c 1 c 2 s 3 c 1 c 2 c 3 − s 1 s 3 ] X 1 Y 2 Z 3 = [ c 2 c 3 − c 2 s 3 s 2 c 1 s 3 + c 3 s 1 s 2 c 1 c 3 − s 1 s 2 s 3 − c 2 s 1 s 1 s 3 − c 1 c 3 s 2 c 3 s 1 + c 1 s 2 s 3 c 1 c 2 ] Y 1 X 2 Y 3 = [ c 1 c 3 − c 2 s 1 s 3 s 1 s 2 c 1 s 3 + c 2 c 3 s 1 s 2 s 3 c 2 − c 3 s 2 − c 3 s 1 − c 1 c 2 s 3 c 1 s 2 c 1 c 2 c 3 − s 1 s 3 ] Y 1 X 2 Z 3 = [ c 1 c 3 + s 1 s 2 s 3 c 3 s 1 s 2 − c 1 s 3 c 2 s 1 c 2 s 3 c 2 c 3 − s 2 c 1 s 2 s 3 − c 3 s 1 c 1 c 3 s 2 + s 1 s 3 c 1 c 2 ] Y 1 Z 2 Y 3 = [ c 1 c 2 c 3 − s 1 s 3 − c 1 s 2 c 3 s 1 + c 1 c 2 s 3 c 3 s 2 c 2 s 2 s 3 − c 1 s 3 − c 2 c 3 s 1 s 1 s 2 c 1 c 3 − c 2 s 1 s 3 ] Y 1 Z 2 X 3 = [ c 1 c 2 s 1 s 3 − c 1 c 3 s 2 c 3 s 1 + c 1 s 2 s 3 s 2 c 2 c 3 − c 2 s 3 − c 2 s 1 c 1 s 3 + c 3 s 1 s 2 c 1 c 3 − s 1 s 2 s 3 ] Z 1 Y 2 Z 3 = [ c 1 c 2 c 3 − s 1 s 3 − c 3 s 1 − c 1 c 2 s 3 c 1 s 2 c 1 s 3 + c 2 c 3 s 1 c 1 c 3 − c 2 s 1 s 3 s 1 s 2 − c 3 s 2 s 2 s 3 c 2 ] Z 1 Y 2 X 3 = [ c 1 c 2 c 1 s 2 s 3 − c 3 s 1 s 1 s 3 + c 1 c 3 s 2 c 2 s 1 c 1 c 3 + s 1 s 2 s 3 c 3 s 1 s 2 − c 1 s 3 − s 2 c 2 s 3 c 2 c 3 ] Z 1 X 2 Z 3 = [ c 1 c 3 − c 2 s 1 s 3 − c 1 s 3 − c 2 c 3 s 1 s 1 s 2 c 3 s 1 + c 1 c 2 s 3 c 1 c 2 c 3 − s 1 s 3 − c 1 s 2 s 2 s 3 c 3 s 2 c 2 ] Z 1 X 2 Y 3 = [ c 1 c 3 − s 1 s 2 s 3 − c 2 s 1 c 1 s 3 + c 3 s 1 s 2 c 3 s 1 + c 1 s 2 s 3 c 1 c 2 s 1 s 3 − c 1 c 3 s 2 − c 2 s 3 s 2 c 2 c 3 ] \begin{array}{l} \begin{array}{c} X_{1} Z_{2} X_{3}=\left[\begin{array}{ccc} c_{2} & -c_{3} s_{2} & s_{2} s_{3} \\ c_{1} s_{2} & c_{1} c_{2} c_{3}-s_{1} s_{3} & -c_{3} s_{1}-c_{1} c_{2} s_{3} \\ s_{1} s_{2} & c_{1} s_{3}+c_{2} c_{3} s_{1} & c_{1} c_{3}-c_{2} s_{1} s_{3} \end{array}\right] \quad X_{1} Z_{2} Y_{3}=\left[\begin{array}{ccc} c_{2} c_{3} & -s_{2} & c_{2} s_{3} \\ s_{1} s_{3}+c_{1} c_{3} s_{2} & c_{1} c_{2} & c_{1} s_{2} s_{3}-c_{3} s_{1} \\ c_{3} s_{1} s_{2}-c_{1} s_{3} & c_{2} s_{1} & c_{1} c_{3}+s_{1} s_{2} s_{3} \end{array}\right] \end{array} \\ X_{1} Y_{2} X_{3}=\left[\begin{array}{ccc} c_{2} & s_{2} s_{3} & c_{3} s_{2} \\ s_{1} s_{2} & c_{1} c_{3}-c_{2} s_{1} s_{3} & -c_{1} s_{3}-c_{2} c_{3} s_{1} \\ -c_{1} s_{2} & c_{3} s_{1}+c_{1} c_{2} s_{3} & c_{1} c_{2} c_{3}-s_{1} s_{3} \end{array}\right] X_{1} Y_{2} Z_{3}=\left[\begin{array}{ccc} c_{2} c_{3} & -c_{2} s_{3} & s_{2} \\ c_{1} s_{3}+c_{3} s_{1} s_{2} & c_{1} c_{3}-s_{1} s_{2} s_{3} & -c_{2} s_{1} \\ s_{1} s_{3}-c_{1} c_{3} s_{2} & c_{3} s_{1}+c_{1} s_{2} s_{3} & c_{1} c_{2} \end{array}\right] \\ Y_{1} X_{2} Y_{3}=\left[\begin{array}{ccc} c_{1} c_{3}-c_{2} s_{1} s_{3} & s_{1} s_{2} & c_{1} s_{3}+c_{2} c_{3} s_{1} \\ s_{2} s_{3} & c_{2} & -c_{3} s_{2} \\ -c_{3} s_{1}-c_{1} c_{2} s_{3} & c_{1} s_{2} & c_{1} c_{2} c_{3}-s_{1} s_{3} \end{array}\right] \quad Y_{1} X_{2} Z_{3}=\left[\begin{array}{ccc} c_{1} c_{3}+s_{1} s_{2} s_{3} & c_{3} s_{1} s_{2}-c_{1} s_{3} & c_{2} s_{1} \\ c_{2} s_{3} & c_{2} c_{3} & -s_{2} \\ c_{1} s_{2} s_{3}-c_{3} s_{1} & c_{1} c_{3} s_{2}+s_{1} s_{3} & c_{1} c_{2} \end{array}\right] \\ Y_{1} Z_{2} Y_{3}=\left[\begin{array}{ccc} c_{1} c_{2} c_{3}-s_{1} s_{3} & -c_{1} s_{2} & c_{3} s_{1}+c_{1} c_{2} s_{3} \\ c_{3} s_{2} & c_{2} & s_{2} s_{3} \\ -c_{1} s_{3}-c_{2} c_{3} s_{1} & s_{1} s_{2} & c_{1} c_{3}-c_{2} s_{1} s_{3} \end{array}\right] \quad Y_{1} Z_{2} X_{3}=\left[\begin{array}{ccc} c_{1} c_{2} & s_{1} s_{3}-c_{1} c_{3} s_{2} & c_{3} s_{1}+c_{1} s_{2} s_{3} \\ s_{2} & c_{2} c_{3} & -c_{2} s_{3} \\ -c_{2} s_{1} & c_{1} s_{3}+c_{3} s_{1} s_{2} & c_{1} c_{3}-s_{1} s_{2} s_{3} \end{array}\right] \\ Z_{1} Y_{2} Z_{3}=\left[\begin{array}{ccc} c_{1} c_{2} c_{3}-s_{1} s_{3} & -c_{3} s_{1}-c_{1} c_{2} s_{3} & c_{1} s_{2} \\ c_{1} s_{3}+c_{2} c_{3} s_{1} & c_{1} c_{3}-c_{2} s_{1} s_{3} & s_{1} s_{2} \\ -c_{3} s_{2} & s_{2} s_{3} & c_{2} \end{array}\right] \quad Z_{1} Y_{2} X_{3}=\left[\begin{array}{ccc} c_{1} c_{2} & c_{1} s_{2} s_{3}-c_{3} s_{1} & s_{1} s_{3}+c_{1} c_{3} s_{2} \\ c_{2} s_{1} & c_{1} c_{3}+s_{1} s_{2} s_{3} & c_{3} s_{1} s_{2}-c_{1} s_{3} \\ -s_{2} & c_{2} s_{3} & c_{2} c_{3} \end{array}\right] \\ Z_{1} X_{2} Z_{3}=\left[\begin{array}{ccc} c_{1} c_{3}-c_{2} s_{1} s_{3} & -c_{1} s_{3}-c_{2} c_{3} s_{1} & s_{1} s_{2} \\ c_{3} s_{1}+c_{1} c_{2} s_{3} & c_{1} c_{2} c_{3}-s_{1} s_{3} & -c_{1} s_{2} \\ s_{2} s_{3} & c_{3} s_{2} & c_{2} \end{array}\right] \quad Z_{1} X_{2} Y_{3}=\left[\begin{array}{ccc} c_{1} c_{3}-s_{1} s_{2} s_{3} & -c_{2} s_{1} & c_{1} s_{3}+c_{3} s_{1} s_{2} \\ c_{3} s_{1}+c_{1} s_{2} s_{3} & c_{1} c_{2} & s_{1} s_{3}-c_{1} c_{3} s_{2} \\ -c_{2} s_{3} & s_{2} & c_{2} c_{3} \end{array}\right] \\ \end{array} X1Z2X3= c2c1s2s1s2−c3s2c1c2c3−s1s3c1s3+c2c3s1s2s3−c3s1−c1c2s3c1c3−c2s1s3 X1Z2Y3= c2c3s1s3+c1c3s2c3s1s2−c1s3−s2c1c2c2s1c2s3c1s2s3−c3s1c1c3+s1s2s3 X1Y2X3= c2s1s2−c1s2s2s3c1c3−c2s1s3c3s1+c1c2s3c3s2−c1s3−c2c3s1c1c2c3−s1s3 X1Y2Z3= c2c3c1s3+c3s1s2s1s3−c1c3s2−c2s3c1c3−s1s2s3c3s1+c1s2s3s2−c2s1c1c2 Y1X2Y3= c1c3−c2s1s3s2s3−c3s1−c1c2s3s1s2c2c1s2c1s3+c2c3s1−c3s2c1c2c3−s1s3 Y1X2Z3= c1c3+s1s2s3c2s3c1s2s3−c3s1c3s1s2−c1s3c2c3c1c3s2+s1s3c2s1−s2c1c2 Y1Z2Y3= c1c2c3−s1s3c3s2−c1s3−c2c3s1−c1s2c2s1s2c3s1+c1c2s3s2s3c1c3−c2s1s3 Y1Z2X3= c1c2s2−c2s1s1s3−c1c3s2c2c3c1s3+c3s1s2c3s1+c1s2s3−c2s3c1c3−s1s2s3 Z1Y2Z3= c1c2c3−s1s3c1s3+c2c3s1−c3s2−c3s1−c1c2s3c1c3−c2s1s3s2s3c1s2s1s2c2 Z1Y2X3= c1c2c2s1−s2c1s2s3−c3s1c1c3+s1s2s3c2s3s1s3+c1c3s2c3s1s2−c1s3c2c3 Z1X2Z3= c1c3−c2s1s3c3s1+c1c2s3s2s3−c1s3−c2c3s1c1c2c3−s1s3c3s2s1s2−c1s2c2 Z1X2Y3= c1c3−s1s2s3c3s1+c1s2s3−c2s3−c2s1c1c2s2c1s3+c3s1s2s1s3−c1c3s2c2c3
[ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] [ 1 0 0 0 cos ϕ − sin ϕ 0 sin ϕ cos ϕ ] = [ c 1 c 2 − s 1 c 1 s 2 s 1 c 2 c 1 s 1 s 2 − s 2 0 c 2 ] [ 1 0 0 0 c 3 − s 3 0 s 3 c 3 ] = [ c 1 c 2 c 1 s 2 s 3 − s 1 c 3 c 1 s 2 s 1 c 2 s 1 s 2 s 3 + c 1 c 3 s 1 s 2 − s 2 c 2 s 3 c 2 ] \begin{aligned} & \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \\ \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix} \\ =& \begin{bmatrix} c_1c_2 & -s_1 & c_1s_2 \\ s_1c_2 & c_1 & s_1s_2 \\ -s_2 & 0 & c_2 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & c_3 & -s_3 \\ 0 & s_3 & c_3 \end{bmatrix} \\ =& \begin{bmatrix} c_1c_2 & c_1s_2s_3-s_1c_3 & c_1s_2 \\ s_1c_2 & s_1s_2s_3+c_1c_3 & s_1s_2 \\ -s_2 & c_2s_3 & c_2 \end{bmatrix} \end{aligned} == cosψsinψ0−sinψcosψ0001 cosθ0−sinθ010sinθ0cosθ 1000cosϕsinϕ0−sinϕcosϕ c1c2s1c2−s2−s1c10c1s2s1s2c2 1000c3s30−s3c3 c1c2s1c2−s2c1s2s3−s1c3s1s2s3+c1c3c2s3c1s2s1s2c2
欧拉角速度
欧拉角速度与角速度的关系推导 -CSDN博客
Angular Velocity expressed via Euler Angles -Physics Stack Exchange
角速度 ω ⃗ \vec\omega ω 是表示在惯性坐标系的，可分解为
ω ⃗ = ω x i ⃗ + ω y j ⃗ + ω z k ⃗ \vec\omega=\omega_x\vec i+\omega_y\vec j+\omega_z\vec k ω=ωxi+ωyj+ωzk
同时，又可以将它分解到刚体固连坐标系三次旋转的转轴上：
ω ⃗ = d ϕ e ⃗ x + d θ e ⃗ y + d ψ e ⃗ z \vec\omega=\text{d}\phi\vec e_x+\text{d}\theta\vec e_y+\text{d}\psi\vec e_z ω=dϕex+dθey+dψez
首先绕 z z z 轴旋转
d ψ e z = [ 1 0 0 0 1 0 0 0 1 ] [ 0 0 d ψ ] \text{d}\psi e_z=\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ \text{d}\psi \end{bmatrix} dψez= 100010001 00dψ
其次绕 y y y 轴旋转
d θ e y = [ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] [ 0 d θ 0 ] \text{d}\theta e_y=\begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} 0 \\ \text{d}\theta \\ 0 \end{bmatrix} dθey= cosψsinψ0−sinψcosψ0001 0dθ0
最后绕 x x x 轴旋转
d ϕ e x = [ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] [ d ϕ 0 0 ] \text{d}\phi e_x=\begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \\ \end{bmatrix}\begin{bmatrix} \text{d}\phi \\ 0 \\ 0 \end{bmatrix} dϕex= cosψsinψ0−sinψcosψ0001 cosθ0−sinθ010sinθ0cosθ dϕ00
将三者相加，就可得到角速度与欧拉角速度的关系
ω ⃗ = R z R y d ϕ ⃗ + R z d θ ⃗ + d ψ ⃗ = I ( d ψ ⃗ + R z ( d θ ⃗ + R y d ϕ ⃗ ) ) = [ cos ψ cos θ d ϕ sin ψ cos θ d ϕ − sin θ d ϕ ] + [ − sin ψ d θ cos ψ d θ 0 ] + [ 0 0 d ψ ] = [ cos ψ cos θ − sin ψ 0 sin ψ cos θ cos ψ 0 − sin θ 0 1 ] [ d ϕ d θ d ψ ] \begin{aligned} \vec\omega =& R_zR_y\text{d}\vec\phi+R_z\text{d}\vec\theta+\text{d}\vec\psi \\ =& I(\text{d}\vec\psi+R_z(\text{d}\vec\theta+R_y\text{d}\vec\phi)) \\ % =& R_x(\text{d}\vec\phi+R_y(\text{d}\vec\theta+R_z\text{d}\vec\psi)) \\ =& \begin{bmatrix} \cos\psi\cos\theta\text{d}\phi \\ \sin\psi\cos\theta\text{d}\phi \\ -\sin\theta\text{d}\phi \\ \end{bmatrix} +\begin{bmatrix} -\sin\psi\text{d}\theta \\ \cos\psi\text{d}\theta \\ 0 \\ \end{bmatrix} +\begin{bmatrix} 0 \\ 0 \\ \text{d}\psi \end{bmatrix} \\ =& \begin{bmatrix} \cos\psi\cos\theta & -\sin\psi & 0 \\ \sin\psi\cos\theta & \cos\psi & 0 \\ -\sin\theta & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} \text{d}\phi \\ \text{d}\theta \\ \text{d}\psi \end{bmatrix} \\ \end{aligned} ω====RzRydϕ+Rzdθ+dψI(dψ+Rz(dθ+Rydϕ)) cosψcosθdϕsinψcosθdϕ−sinθdϕ + −sinψdθcosψdθ0 + 00dψ cosψcosθsinψcosθ−sinθ−sinψcosψ0001 dϕdθdψ
反过来得到
[ d ϕ d θ d ψ ] = [ cos ψ / cos θ − sin ψ / cos θ 0 sin ψ cos ψ 0 − c o s ψ tan θ sin ψ tan θ 1 ] ω ⃗ \begin{aligned} \begin{bmatrix} \text{d}\phi \\ \text{d}\theta \\ \text{d}\psi \end{bmatrix} = \begin{bmatrix} \cos\psi/\cos\theta & -\sin\psi/\cos\theta & 0 \\ \sin\psi & \cos\psi & 0 \\ -cos\psi\tan\theta & \sin\psi\tan\theta & 1 \\ \end{bmatrix}\vec\omega \end{aligned} dϕdθdψ = cosψ/cosθsinψ−cosψtanθ−sinψ/cosθcosψsinψtanθ001 ω
xyz固定角和zyx欧拉角相等的直观理解
xyz固定角指所有的旋转均在世界坐标系下，刚体先后绕世界坐标系的xyz轴旋转，设旋转前的向量为 v ⃗ \vec{v} v (为了便于理解，可以想象 v ⃗ = ( 1 , 0 , 0 ) T \vec{v}=(1,0,0)^T v=(1,0,0)T)，则旋转后的向量为
v ⃗ 1 = R z R y R x v ⃗ \vec{v}_1=R_zR_yR_x\vec{v} v1=RzRyRxv
zyx欧拉角指3次旋转均为绕与刚体固连的坐标系的轴旋转，顺序为zyx。旋转前刚体坐标系与世界坐标系重合，绕刚体坐标系z轴旋转一次后的向量在世界坐标系下的坐标为 R z v ⃗ R_z\vec{v} Rzv。一直在动的刚体坐标系称为刚体系，刚体系在绕z轴旋转一次后的坐标系称为"刚1系"。此时再绕刚体系(刚1系)y轴旋转，旋转后的坐标在刚体系中仍为 v ⃗ \vec{v} v，在刚1系中的坐标为 R y v ⃗ R_y\vec{v} Ryv。此时注意对向量乘一个矩阵的另一个作用，即将向量从一个坐标系变换到另一个坐标系，刚1系下的任意坐标 x ⃗ \vec{x} x 在世界系下的坐标为 R z x ⃗ R_z\vec{x} Rzx，所以现在旋转了两次的向量 v ⃗ \vec{v} v 在刚1系中的坐标为 R y v ⃗ R_y\vec{v} Ryv，在世界系中的坐标就是 R z R y v ⃗ R_zR_y\vec{v} RzRyv。
同理，再旋转最后一次后，向量 v ⃗ \vec{v} v 在刚体系的坐标仍然为 v ⃗ \vec{v} v，在刚2系中的坐标为 R x v ⃗ R_x\vec{v} Rxv，刚2系的任意坐标 x ⃗ \vec{x} x 在刚1系下的坐标为 R y x ⃗ R_y\vec{x} Ryx 所以向量 v ⃗ \vec{v} v 在刚2系中的坐标为 R x v ⃗ R_x\vec{v} Rxv，在刚1系中的坐标为 R y R x v ⃗ R_yR_x\vec{v} RyRxv，在世界系中的坐标为 R z R y R x v ⃗ R_zR_yR_x\vec{v} RzRyRxv，与xyz固定角相同。
- John J. Craig. 机器人学导论.
其他参考
- 理解四元数 -Wyman的原创技术博客
- Understanding Quaternions -3D Game Engine Programming
- 四元数——基本概念 -知乎
- 如何形象地理解四元数？ -知乎
- 四元数与三维旋转 -知乎
- Quaternions and Rotations -stanford