自用的四元数、欧拉角、旋转矩阵笔记

编程入门行业动态更新时间:2024-10-03 08:18:54

自用的四元数、欧拉角、旋转矩阵笔记

四元数

单位四元数
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+k2 sin2θ=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

四元数微分方程

[ q ˙ 0 q ˙ 1 q ˙ 2 q ˙ 3 ] = 1 2 [ 0 − ω x − ω y − ω z ω x 0 − ω z ω y ω y − ω z 0 − ω x ω z ω y − ω x 0 ] [ q 0 q 1 q 2 q 3 ] = 1 2 [ q 0 − q 1 − q 2 − q 3 q 1 q 0 q 3 − q 2 q 2 − q 3 q 0 q 1 q 3 q 2 − q 1 q 0 ] [ 0 ω x ω y ω z ] \begin{aligned} \begin{bmatrix} \dot{q}_0 \\ \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \\ \end{bmatrix} =& \frac{1}{2}\begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & -\omega_z & \omega_y \\ \omega_y & -\omega_z & 0 & -\omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} \\ =& \frac{1}{2}\begin{bmatrix} q_{0} & -q_{1} & -q_{2} & -q_{3} \\ q_{1} & q_{0} & q_{3} & -q_{2} \\ q_{2} & -q_{3} & q_{0} & q_{1} \\ q_{3} & q_{2} & -q_{1} & q_{0} \end{bmatrix}\begin{bmatrix} 0 \\ \omega_{x} \\ \omega_{y} \\ \omega_{z} \end{bmatrix} \end{aligned} q˙0q˙1q˙2q˙3 ==21 0ωxωyωz−ωx0−ωzωy−ωy−ωz0−ωx−ωzωy−ωx0 q0q1q2q3 21 q0q1q2q3−q1q0−q3q2−q2q3q0−q1−q3−q2q1q0 0ωxωyωz

旋转矩阵

横滚角 ϕ \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θ)n n T+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轴的旋转顺序，先绕z轴偏航，再绕y轴俯仰，最后绕x轴横滚。本体系x轴朝后为横滚轴，向左横滚为正；y轴朝右为俯仰轴，抬头为正；z轴朝上为偏航轴，向左偏航为正。旋转后横滚俯仰偏航三个角度的定义见后文四元数、旋转矩阵转欧拉角一节。

四元数转旋转矩阵

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
Quaternion to Rotation Matrix -OpenGL

旋转矩阵转四元数

Conversion of rotation matrix to quaternion -stackexchange
Converting a Rotation Matrix to a Quaternion -stackexchange
Maths - Conversion Matrix to Quaternion -EuclideanSpace

旋转矩阵与四元数的各个元素间有如下关系
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
已知旋转矩阵求四元数，上面方程组可以等价写为已知 a 1 a_1 a1 到 a 9 a_9 a9 求 x , y , z , w x,y,z,w x,y,z,w：
4 x 2 = a 1 4 y 2 = a 2 4 z 2 = a 3 4 x y = a 4 4 x z = a 5 4 y z = a 6 4 w z = a 7 4 w y = a 8 4 w x = a 9 4x^2=a_1 \\ 4y^2=a_2 \\ 4z^2=a_3 \\ 4xy=a_4 \\ 4xz=a_5 \\ 4yz=a_6 \\ 4wz=a_7 \\ 4wy=a_8 \\ 4wx=a_9 \\ 4x2=a14y2=a24z2=a34xy=a44xz=a54yz=a64wz=a74wy=a84wx=a9
这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
例如，
x = a 1 4 x = a 1 ± 2 4 x 2 = a 1 ± 2 a 1 y = a 4 4 x = a 4 ± 2 4 x 2 = a 4 ± 2 a 1 z = a 5 4 x = a 5 ± 2 4 x 2 = a 5 ± 2 a 1 w = a 9 4 x = a 9 ± 2 4 x 2 = a 9 ± 2 a 1 x=\frac{a_1}{4x}=\frac{a_1}{\pm 2\sqrt{4x^2}}=\frac{a_1}{\pm 2\sqrt{a_1}} \\ y=\frac{a_4}{4x}=\frac{a_4}{\pm 2\sqrt{4x^2}}=\frac{a_4}{\pm 2\sqrt{a_1}} \\ z=\frac{a_5}{4x}=\frac{a_5}{\pm 2\sqrt{4x^2}}=\frac{a_5}{\pm 2\sqrt{a_1}} \\ w=\frac{a_9}{4x}=\frac{a_9}{\pm 2\sqrt{4x^2}}=\frac{a_9}{\pm 2\sqrt{a_1}} \\ x=4xa1=±24x2 a1=±2a1 a1y=4xa4=±24x2 a4=±2a1 a4z=4xa5=±24x2 a5=±2a1 a5w=4xa9=±24x2 a9=±2a1 a9
这个例子以共同项 4 x 4x 4x 或 a 1 a_1 a1 为分母，但有时候分母可能为0，所以需要根据实际情况分别以4个共同项为分母。共同项 4 y 4y 4y、 4 z 4z 4z 与 4 x 4x 4x 类似，而 4 w 4w 4w 的计算方法为，将前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
实际中有一些比较常用的特殊情况。例如，四元数
Q 1 = [ 0 , 1 , 0 , 0 ] Q 2 = [ 0 , 0 , 1 , 0 ] Q 3 = [ 0 , 0 , 0 , 1 ] Q_1=[0, 1, 0, 0] \\ Q_2=[0, 0, 1, 0] \\ Q_3=[0, 0, 0, 1] \\ Q1=[0,1,0,0]Q2=[0,0,1,0]Q3=[0,0,0,1]
对应的旋转矩阵分别为
[ 1 0 0 0 − 1 0 0 0 − 1 ] [ − 1 0 0 0 1 0 0 0 − 1 ] [ − 1 0 0 0 − 1 0 0 0 1 ] \begin{bmatrix} 1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \begin{bmatrix} -1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \begin{bmatrix} -1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & 1 \end{bmatrix} 1000−1000−1 −10001000−1 −1000−10001
四元数
[ 0.5 , 0.5 , 0.5 , 0.5 ] [ 0.5 , − 0.5 , 0.5 , 0.5 ] [ 0.5 , 0.5 , − 0.5 , 0.5 ] [ 0.5 , 0.5 , 0.5 , − 0.5 ] [0.5, 0.5, 0.5, 0.5] \\ [0.5, -0.5, 0.5, 0.5] \\ [0.5, 0.5, -0.5, 0.5] \\ [0.5, 0.5, 0.5, -0.5] \\ [0.5,0.5,0.5,0.5][0.5,−0.5,0.5,0.5][0.5,0.5,−0.5,0.5][0.5,0.5,0.5,−0.5]
[ − 0.5 , 0.5 , 0.5 , 0.5 ] [ − 0.5 , − 0.5 , 0.5 , 0.5 ] [ − 0.5 , 0.5 , − 0.5 , 0.5 ] [-0.5, 0.5, 0.5, 0.5] \\ [-0.5, -0.5, 0.5, 0.5] \\ [-0.5, 0.5, -0.5, 0.5] \\ [−0.5,0.5,0.5,0.5][−0.5,−0.5,0.5,0.5][−0.5,0.5,−0.5,0.5]
对应的旋转矩阵分别为
[ 0 0 1 1 0 0 0 1 0 ] [ 0 − 1 0 0 0 1 − 1 0 0 ] [ 0 − 1 0 0 0 − 1 1 0 0 ] [ 0 1 0 0 0 − 1 − 1 0 0 ] [ 0 1 0 0 0 1 1 0 0 ] [ 0 0 − 1 − 1 0 0 0 1 0 ] [ 0 0 1 − 1 0 0 0 − 1 0 ] \begin{bmatrix} 0 & 0 & 1 \\ 1 & 0 & 0 \\ 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} 0 & -1 & 0 \\ 0 & 0 & 1 \\ -1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & -1 \\ -1 & 0 & 0 \end{bmatrix} \\ \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & -1 \\ -1 & 0 & 0 \\ 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & 1 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{bmatrix} 010001100 00−1−100010 001−1000−10 00−11000−10 001100010 0−10001−100 0−1000−1100

如上图所示，左边是世界系，右边是本体系，可以看出本体系到世界系的旋转矩阵为
[ 0 0 1 − 1 0 0 0 − 1 0 ] \begin{bmatrix} 0 & 0 & 1 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{bmatrix} 0−1000−1100

欧拉角转旋转矩阵

[ 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

四元数、旋转矩阵转欧拉角

航模飞控上用的四元数转欧拉角代码如下

rol=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
pit=asin(2*(q0*q2-q1*q3));
yaw=atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

四元数转旋转矩阵代码

Mat matqr(3, 3, vecdble{q0*q0+q1*q1-q2*q2-q3*q3, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2,2*q1*q2+2*q0*q3, q0*q0-q1*q1+q2*q2-q3*q3, 2*q2*q3-2*q0*q1,2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3,
});

对应得到旋转矩阵转欧拉角

rol=atan2(r32, r33);
pit=asin(-r31);
yaw=atan2(r21, r11);

二值反正切函数为atan2(y,x)，横滚角的二值反正切函数中的y、x值分别为本体系y坐标轴和z坐标轴在世界系的z坐标(r23, r33)，俯仰角为0时也可以理解成世界系z轴在本体系YOZ平面上的投影与z轴的夹角（但俯仰角不为0时不能这样等价）。偏航角的二值反正切函数中的y、x值分别为本体系x轴在世界系的y坐标和x坐标(r21, r11)，可以理解成本体系x轴在世界系XOY平面上的投影与世界系x轴的夹角。俯仰角为本体系x轴与世界系XOY平面的距离，因为x轴朝向本体后方，所以取负号，表示抬头为正。

欧拉角速度

欧拉角速度与角速度的关系推导 -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ϕe x+dθe y+dψe z
首先绕 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 ω

罗德里格斯参数

g = 1 q w [ q x q y q z ] = e tan ⁡ Φ 2 g=\frac{1}{q_w}\begin{bmatrix} q_x \\ q_y \\ q_z \end{bmatrix}=e\tan\frac{\Phi}{2} g=qw1 qxqyqz =etan2Φ

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} v 1=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固定角相同。