四元数
Q = ⟨ x , y , z , w ⟩ Q = \langle x, y, z, w \rangle Q=⟨x,y,z,w⟩
Q = x i + y j + z k + w Q = xi + yj + zk + w Q=xi+yj+zk+w
1.运算与性质
1)数乘
Q = s ⋅ ⟨ x , y , z , w ⟩ = ⟨ s ⋅ z , s ⋅ y , s ⋅ z , s ⋅ w ⟩ Q = s \cdot \langle x, y, z, w \rangle = \langle s \cdot z, s \cdot y, s \cdot z, s \cdot w \rangle Q=s⋅⟨x,y,z,w⟩=⟨s⋅z,s⋅y,s⋅z,s⋅w⟩
Q s = ( x s , y s , z s , w s ) \frac{Q}{s} = \left( \frac{x}{s}, \frac{y}{s}, \frac{z}{s}, \frac{w}{s} \right) sQ=(sx,sy,sz,sw)
2)四元数加减法(Add,Substract)
Q = ⟨ x 1 + x 2 , y 1 + y 2 , z 1 + z 2 , w 1 + w 2 ⟩ Q = \langle x_1 + x_2, y_1 + y_2, z_1 + z_2, w_1 + w_2 \rangle Q=⟨x1+x2,y1+y2,z1+z2,w1+w2⟩
Q = ⟨ x 1 − x 2 , y 1 − y 2 , z 1 − z 2 , w 1 − w 2 ⟩ Q = \langle x_1 - x_2, y_1 - y_2, z_1 - z_2, w_1 - w_2 \rangle Q=⟨x1−x2,y1−y2,z1−z2,w1−w2⟩
3) 模运算(Norm)
∥ Q ∥ = x 2 + y 2 + z 2 + w 2 \|Q\| = \sqrt{x^2 + y^2 + z^2 + w^2} ∥Q∥=x2+y2+z2+w2
4)标准化(Normalize)
Q normalized = Q ∥ Q ∥ = ⟨ x , y , z , w ⟩ x 2 + y 2 + z 2 + w 2 Q_{\text{normalized}} = \frac{Q}{\|Q\|} = \frac{\langle x, y, z, w \rangle}{\sqrt{x^2 + y^2 + z^2 + w^2}} Qnormalized=∥Q∥Q=x2+y2+z2+w2⟨x,y,z,w⟩
5)共轭(Conjugate)
Q ∗ = ⟨ − x , − y , − z , w ⟩ Q^* = \langle -x, -y, -z, w \rangle Q∗=⟨−x,−y,−z,w⟩
6)逆元 (Reverse)
Q − 1 = Q ∗ ∥ Q ∥ 2 Q^{-1} = \frac{Q^*}{\|Q\|^2} Q−1=∥Q∥2Q∗
7)四元数乘法(Mutiply)
Q 1 ∗ Q 2 = ( w 1 w 2 − x 1 x 2 − y 1 y 2 − z 1 z 2 , w 1 x 2 + x 1 w 2 + y 1 z 2 − z 1 y 2 , w 1 y 2 − x 1 z 2 + y 1 w 2 + z 1 x 2 , w 1 z 2 + x 1 y 2 − y 1 x 2 + z 1 w 2 ) \begin{align*} Q_1 * Q_2 = \left( w_1w_2 - x_1x_2 - y_1y_2 - z_1z_2, \right. \\ \left. w_1x_2 + x_1w_2 + y_1z_2 - z_1y_2, \right. \\ \left. w_1y_2 - x_1z_2 + y_1w_2 + z_1x_2, \right. \\ \left. w_1z_2 + x_1y_2 - y_1x_2 + z_1w_2 \right) \end{align*} Q1∗Q2=(w1w2−x1x2−y1y2−z1z2,w1x2+x1w2+y1z2−z1y2,w1y2−x1z2+y1w2+z1x2,w1z2+x1y2−y1x2+z1w2)
注意:在下文我们使用 · (点) 来表示点乘或者数乘,使用 * (星)表示四元数乘法
7)欧拉角构造四元数
在欧拉角的值为角度时
Yaw ( ψ ) → ψ Pitch ( θ ) → θ Roll ( ϕ ) → ϕ \begin{align*} \text{Yaw (}\psi\text{)} &\rightarrow \psi \\ \text{Pitch (}\theta\text{)} &\rightarrow \theta \\ \text{Roll (}\phi\text{)} &\rightarrow \phi \\ \end{align*} Yaw (ψ)Pitch (θ)Roll (ϕ)→ψ→θ→ϕ
在值为弧度时
Yaw ( ψ ) → Yaw ( ψ ) ⋅ π 180 Pitch ( θ ) → Pitch ( θ ) ⋅ π 180 Roll ( ϕ ) → Roll ( ϕ ) ⋅ π 180 \begin{align*} \text{Yaw (}\psi\text{)} &\rightarrow \frac{\text{Yaw (}\psi\text{)} \cdot \pi}{180} \\ \text{Pitch (}\theta\text{)} &\rightarrow \frac{\text{Pitch (}\theta\text{)} \cdot \pi}{180} \\ \text{Roll (}\phi\text{)} &\rightarrow \frac{\text{Roll (}\phi\text{)} \cdot \pi}{180} \\ \end{align*} Yaw (ψ)Pitch (θ)Roll (ϕ)→180Yaw (ψ)⋅π→180Pitch (θ)⋅π→180Roll (ϕ)⋅π
变化后,求得四元数的四个分量
x = sin ( ψ 2 ) ⋅ cos ( θ 2 ) ⋅ cos ( ϕ 2 ) − cos ( ψ 2 ) ⋅ sin ( θ 2 ) ⋅ sin ( ϕ 2 ) y = cos ( ψ 2 ) ⋅ sin ( θ 2 ) ⋅ cos ( ϕ 2 ) + sin ( ψ 2 ) ⋅ cos ( θ 2 ) ⋅ sin ( ϕ 2 ) z = cos ( ψ 2 ) ⋅ cos ( θ 2 ) ⋅ sin ( ϕ 2 ) − sin ( ψ 2 ) ⋅ sin ( θ 2 ) ⋅ cos ( ϕ 2 ) w = cos ( ψ 2 ) ⋅ cos ( θ 2 ) ⋅ cos ( ϕ 2 ) + sin ( ψ 2 ) ⋅ sin ( θ 2 ) ⋅ sin ( ϕ 2 ) \begin{align*} x &= \sin\left(\frac{\psi}{2}\right) \cdot \cos\left(\frac{\theta}{2}\right) \cdot \cos\left(\frac{\phi}{2}\right) - \cos\left(\frac{\psi}{2}\right) \cdot \sin\left(\frac{\theta}{2}\right) \cdot \sin\left(\frac{\phi}{2}\right) \\ y &= \cos\left(\frac{\psi}{2}\right) \cdot \sin\left(\frac{\theta}{2}\right) \cdot \cos\left(\frac{\phi}{2}\right) + \sin\left(\frac{\psi}{2}\right) \cdot \cos\left(\frac{\theta}{2}\right) \cdot \sin\left(\frac{\phi}{2}\right) \\ z &= \cos\left(\frac{\psi}{2}\right) \cdot \cos\left(\frac{\theta}{2}\right) \cdot \sin\left(\frac{\phi}{2}\right) - \sin\left(\frac{\psi}{2}\right) \cdot \sin\left(\frac{\theta}{2}\right) \cdot \cos\left(\frac{\phi}{2}\right) \\ w &= \cos\left(\frac{\psi}{2}\right) \cdot \cos\left(\frac{\theta}{2}\right) \cdot \cos\left(\frac{\phi}{2}\right) + \sin\left(\frac{\psi}{2}\right) \cdot \sin\left(\frac{\theta}{2}\right) \cdot \sin\left(\frac{\phi}{2}\right) \\ \end{align*} xyzw=sin(2ψ)⋅cos(2θ)⋅cos(2ϕ)−cos(2ψ)⋅sin(2θ)⋅sin(2ϕ)=cos(2ψ)⋅sin(2θ)⋅cos(2ϕ)+sin(2ψ)⋅cos(2θ)⋅sin(2ϕ)=cos(2ψ)⋅cos(2θ)⋅sin(2ϕ)−sin(2ψ)⋅sin(2θ)⋅cos(2ϕ)=cos(2ψ)⋅cos(2θ)⋅cos(2ϕ)+sin(2ψ)⋅sin(2θ)⋅sin(2ϕ)
如果需要对某个轴旋转特定角度
Q = ( sin ( θ 2 ) ⋅ a , sin ( θ 2 ) ⋅ b , sin ( θ 2 ) ⋅ c , cos ( θ 2 ) ) Q = \left(\sin\left(\frac{\theta}{2}\right) \cdot a, \sin\left(\frac{\theta}{2}\right) \cdot b, \sin\left(\frac{\theta}{2}\right) \cdot c, \cos\left(\frac{\theta}{2}\right)\right) Q=(sin(2θ)⋅a,sin(2θ)⋅b,sin(2θ)⋅c,cos(2θ))
8)四元数转欧拉角
Yaw ( ψ ) → arctan 2 ( 2 ⋅ ( x ⋅ y + z ⋅ w ) , 1 − 2 ⋅ ( y 2 + z 2 ) ) Pitch ( θ ) → arcsin ( 2 ⋅ ( x ⋅ z − w ⋅ y ) ) Roll ( ϕ ) → arctan 2 ( 2 ⋅ ( y ⋅ z + x ⋅ w ) , 1 − 2 ⋅ ( z 2 + y 2 ) ) \begin{align*} \text{Yaw (}\psi\text{)} &\rightarrow \arctan2(2 \cdot (x \cdot y + z \cdot w), 1 - 2 \cdot (y^2 + z^2)) \\ \text{Pitch (}\theta\text{)} &\rightarrow \arcsin(2 \cdot (x \cdot z - w \cdot y)) \\ \text{Roll (}\phi\text{)} &\rightarrow \arctan2(2 \cdot (y \cdot z + x \cdot w), 1 - 2 \cdot (z^2 + y^2)) \end{align*} Yaw (ψ)Pitch (θ)Roll (ϕ)→arctan2(2⋅(x⋅y+z⋅w),1−2⋅(y2+z2))→arcsin(2⋅(x⋅z−w⋅y))→arctan2(2⋅(y⋅z+x⋅w),1−2⋅(z2+y2))
(注意:arctan2是一个被多种编程语言支持的反正切函数)
9)四元数点乘
Q 1 ⋅ Q 2 = x 1 ⋅ x 2 + y 1 ⋅ y 2 + z 1 ⋅ z 2 + w 1 ⋅ w 2 Q_1 \cdot Q_2 = x_1 \cdot x_2 + y_1 \cdot y_2 + z_1 \cdot z_2 + w_1 \cdot w_2 Q1⋅Q2=x1⋅x2+y1⋅y2+z1⋅z2+w1⋅w2
我们可以通过这样的方式计算出两个四元数的夹角
θ = arccos ( Q 1 ⋅ Q 2 ) \theta = \arccos(Q_1 \cdot Q_2) θ=arccos(Q1⋅Q2)
当点积为0时,表明两个四元数正交
Q 1 ⋅ Q 2 = 0 Q_1 \cdot Q_2 = 0 Q1⋅Q2=0
当四元数保持单位模时,表明为单位四元数
Q ⋅ Q = 1 Q \cdot Q = 1 Q⋅Q=1
10)四元数普通线性插值
Lerp的公式如下
Q lerp = ( 1 − t ) ⋅ Q 1 + t ⋅ Q 2 Q_{\text{lerp}} = (1 - t) \cdot Q_1 + t \cdot Q_2 Qlerp=(1−t)⋅Q1+t⋅Q2
这个插值被称为NLerp,也就是先进行线性插值,再进行标准化
Q nlerp = ( 1 − t ) ⋅ Q 1 + t ⋅ Q 2 ∥ ( 1 − t ) ⋅ Q 1 + t ⋅ Q 2 ∥ Q_{\text{nlerp}} = \frac{(1 - t) \cdot Q_1 + t \cdot Q_2}{\| (1 - t) \cdot Q_1 + t \cdot Q_2 \|} Qnlerp=∥(1−t)⋅Q1+t⋅Q2∥(1−t)⋅Q1+t⋅Q2
注意:此处的“点”均为“数乘”或“四元数点乘”
11) 四元数球面线性插值
α = cos − 1 ( Q 1 ⋅ Q 2 ) \alpha = \cos^{-1}(Q_1 \cdot Q_2) α=cos−1(Q1⋅Q2)
Q slerp = = sin ( ( 1 − t ) ⋅ α ) sin ( α ) ⋅ Q 1 + sin ( t ⋅ α ) sin ( α ) ⋅ Q 2 Q_{\text{slerp}} = = \frac{\sin((1 - t) \cdot \alpha)}{\sin(\alpha)} \cdot Q_1 + \frac{\sin(t \cdot \alpha)}{\sin(\alpha)} \cdot Q_2 Qslerp==sin(α)sin((1−t)⋅α)⋅Q1+sin(α)sin(t⋅α)⋅Q2
注意:此处的“点”均为“数乘”或“四元数点乘”
12) 四元数转旋转矩阵
仿射变换旋转矩阵:
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 ] 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} R=
1−2y2−2z22xy+2wz2xz−2wy2xy−2wz1−2x2−2z22yz+2wx2xz+2wy2yz−2wx1−2x2−2y2
齐次坐标旋转矩阵:
T = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 w z 2 x z + 2 w y 0 2 x y + 2 w z 1 − 2 x 2 − 2 z 2 2 y z − 2 w x 0 2 x z − 2 w y 2 y z + 2 w x 1 − 2 x 2 − 2 y 2 0 0 0 0 1 ] T = \begin{bmatrix} 1 - 2y^2 - 2z^2 & 2xy - 2wz & 2xz + 2wy & 0 \\ 2xy + 2wz & 1 - 2x^2 - 2z^2 & 2yz - 2wx & 0 \\ 2xz - 2wy & 2yz + 2wx & 1 - 2x^2 - 2y^2 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T=
1−2y2−2z22xy+2wz2xz−2wy02xy−2wz1−2x2−2z22yz+2wx02xz+2wy2yz−2wx1−2x2−2y200001
12)旋转矩阵转四元数
对于以下仿射变换旋转矩阵
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
或在齐次坐标下的旋转矩阵
T = [ R 11 R 12 R 13 0 R 21 R 22 R 23 0 R 31 R 32 R 33 0 0 0 0 1 ] T = \begin{bmatrix} R_{11} & R_{12} & R_{13} & 0 \\ R_{21} & R_{22} & R_{23} & 0 \\ R_{31} & R_{32} & R_{33} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T=
R11R21R310R12R22R320R13R23R3300001
都可以按以下方式获取四元数的四个部分
Q = { w = 1 + R 11 + R 22 + R 33 x = R 32 − R 23 4 w y = R 13 − R 31 4 w z = R 21 − R 12 4 w Q = \begin{cases} w = \sqrt{1 + R_{11} + R_{22} + R_{33}} \\ x = \frac{R_{32} - R_{23}}{4w} \\ y = \frac{R_{13} - R_{31}}{4w} \\ z = \frac{R_{21} - R_{12}}{4w} \end{cases} Q=⎩
⎨
⎧w=1+R11+R22+R33x=4wR32−R23y=4wR13−R31z=4wR21−R12
12)四元数叉乘
一般来讲四元数叉乘并没有意义,只是在某些特定的算法或情景中定义的全新的运算法则,而不具备广泛的实用性。可以根据需求调整叉乘的定义,这里不再给出相关信息。
2.运算律与定义
1).单位元(Identity)
规定单位元为
Q i d e n t i t y = ⟨ 0 , 0 , 0 , 1 ⟩ Q_{identity} = \langle 0, 0, 0, 1 \rangle Qidentity=⟨0,0,0,1⟩
2).单位四元数
规定模长(Norm)为1为单位四元数
3).逆元
每个非零四元数都有一个逆元,相乘结果为单位元,注意前置条件
Q ⋅ Q − 1 = Q i d e n t i t y Q \cdot Q^{-1}= Q_{identity} Q⋅Q−1=Qidentity
4).结合律
( Q 1 ⋅ Q 2 ) ⋅ Q 3 = Q 1 ⋅ ( Q 2 ⋅ Q 3 ) (Q_1 \cdot Q_2) \cdot Q_3 = Q_1 \cdot (Q_2 \cdot Q_3) (Q1⋅Q2)⋅Q3=Q1⋅(Q2⋅Q3)
5).乘法交换率不成立
Q 1 ⋅ Q 2 ≠ Q 2 ⋅ Q 1 Q_1 \cdot Q_2 \neq Q_2 \cdot Q_1 Q1⋅Q2=Q2⋅Q1
6).乘法封闭性
四元数乘法是封闭的,也就是说,两个四元数的乘积仍然是四元数。
7).无万向节锁问题
"万向锁问题"是指在使用欧拉角来描述旋转时,存在一个特定情况,其中三个旋转轴的旋转角度不能独立地表示物体的方向,因为它们会相互影响。这会导致旋转的奇异性,使得无法准确描述物体的姿态。
四元数和旋转矩阵均无万向锁问题,但是矩阵需要占据很大的运算空间,有高昂的代价,而四元数解决这一问题的代价是升维,使得旋转失去了直观性和简单性。