罗德里格旋转公式(Rodrigues’ rotation formula) 是计算三维空间中,一个向量绕旋转轴旋转给定角度以后得到的新向量的计算公式。被广泛应用于空间解析几何和计算机图形学领域,成为刚体运动的基本计算公式。
1.旋转轴
k
⃗
\vec k
k与旋转向量
v
⃗
\vec v
v垂直,则
v
⃗
r
o
t
=
c
o
s
θ
⋅
v
⃗
+
s
i
n
θ
⋅
(
k
⃗
×
v
⃗
)
\vec v_{rot}=cos\theta\cdot\vec{v}+sin\theta\cdot(\vec k\times\vec v)
vrot=cosθ⋅v+sinθ⋅(k×v)
推导过程:
对旋转后的向量进行正交分解: v ⃗ r o t = v 1 ⃗ + v 2 ⃗ \vec v_{rot}=\vec{v_1}+\vec{v_2} vrot=v1+v2
因为 v 1 ⃗ = ∣ v 1 ⃗ ∣ ⋅ i ⃗ \vec{v_1}=|\vec{v_1}|\cdot \vec i v1=∣v1∣⋅i ,其中 i i i 是 v ⃗ 1 \vec v_1 v1 的单位向量,所以 v 1 ⃗ = ∣ v ⃗ ∣ ⋅ c o s θ ⋅ v ⃗ ∣ v ⃗ ∣ = c o s θ ⋅ v ⃗ \vec{v_1}=|\vec{v}|\cdot cos\theta\cdot\frac{\vec v}{|\vec v|}=cos\theta\cdot{\vec v} v1=∣v∣⋅cosθ⋅∣v∣v=cosθ⋅v
同理 v 2 ⃗ = ∣ v ⃗ ∣ ⋅ s i n θ ⋅ k ⃗ × v ⃗ ∣ k ⃗ ∣ ∣ v ⃗ ∣ s i n < k ⃗ , v ⃗ > = s i n θ ⋅ k ⃗ × v ⃗ \vec{v_2}=|\vec{v}|\cdot sin\theta\cdot\frac{\vec k \times \vec v}{|\vec k||\vec v|sin<\vec k,\vec v>}=sin\theta\cdot{\vec k\times \vec v} v2=∣v∣⋅sinθ⋅∣k∣∣v∣sin<k,v>k×v=sinθ⋅k×v.
2.旋转轴
k
⃗
\vec k
k与旋转向量
v
⃗
\vec v
v不垂直,则
v
⃗
r
o
t
=
c
o
s
θ
⋅
v
⃗
+
(
1
−
c
o
s
θ
)
(
v
⃗
⋅
k
⃗
)
k
⃗
+
s
i
n
θ
⋅
(
k
⃗
×
v
⃗
)
\vec v_{rot}=cos\theta\cdot\vec{v}+(1-cos\theta)(\vec v\cdot\vec k)\vec k+sin\theta\cdot(\vec k\times\vec v)
vrot=cosθ⋅v+(1−cosθ)(v⋅k)k+sinθ⋅(k×v)
推导如下:
对 v ⃗ \vec v v做正交分解, v ⃗ = v ∣ ∣ ⃗ + v ⊥ ⃗ \vec v=\vec{v_{||}}+\vec{v_{\bot} } v=v∣∣+v⊥
v ∣ ∣ ⃗ = ∣ v ⃗ ∣ c o s < v ⃗ , k ⃗ > ⋅ k ⃗ = ∣ v ⃗ ∣ v ⃗ ⋅ k ⃗ ∣ v ⃗ ∣ ⋅ ∣ k ⃗ ∣ ⋅ k ⃗ = ( v ⃗ ⋅ k ⃗ ) k ⃗ \vec{v_{||}}=|\vec v|cos<\vec v,\vec k>\cdot \vec{k}=|\vec v|\frac {\vec v \cdot \vec k}{|\vec v| \cdot |\vec k|}\cdot \vec{k}=(\vec v \cdot \vec k)\vec{k} v∣∣=∣v∣cos<v,k>⋅k=∣v∣∣v∣⋅∣k∣v⋅k⋅k=(v⋅k)k ;
v ⃗ ⊥ = v ⃗ − v ∣ ∣ ⃗ \vec v_{\bot}=\vec v-\vec{v_{||}} v⊥=v−v∣∣;
由1得: v ⃗ ⊥ r o t = c o s θ ⋅ v ⃗ ⊥ + s i n θ ⋅ ( k ⃗ × v ⃗ ⊥ ) \vec v_{\bot rot}=cos\theta\cdot\vec{v}_{\bot}+sin\theta\cdot(\vec k\times\vec v_{\bot}) v⊥rot=cosθ⋅v⊥+sinθ⋅(k×v⊥);
对旋转后的向量进行正交分解:
v
⃗
r
o
t
=
v
∣
∣
⃗
+
v
⊥
r
o
t
⃗
=
(
v
⃗
⋅
k
⃗
)
k
⃗
+
c
o
s
θ
⋅
v
⃗
⊥
+
s
i
n
θ
⋅
(
k
⃗
×
v
⃗
⊥
)
=
(
v
⃗
⋅
k
⃗
)
k
⃗
+
c
o
s
θ
⋅
(
v
⃗
−
(
v
⃗
⋅
k
⃗
)
k
⃗
)
+
s
i
n
θ
⋅
(
k
⃗
×
(
v
⃗
−
v
∣
∣
⃗
)
)
=
c
o
s
θ
⋅
v
⃗
+
(
1
−
c
o
s
θ
)
⋅
(
v
⃗
⋅
k
⃗
)
k
⃗
+
s
i
n
θ
⋅
(
k
⃗
×
v
⃗
)
\begin{aligned} \vec v_{rot}&=\vec{v_{||}}+\vec{v_{\bot rot}}\\ &=(\vec v \cdot \vec k)\vec{k}+cos\theta\cdot\vec{v}_{\bot}+sin\theta\cdot(\vec k\times\vec v_{\bot})\\ &=(\vec v \cdot \vec k)\vec{k}+cos\theta\cdot(\vec v-(\vec v \cdot \vec k)\vec{k} )+sin\theta\cdot(\vec k\times(\vec v-\vec{v_{||}}))\\ &=cos\theta\cdot\vec v+(1-cos\theta)\cdot(\vec v \cdot \vec k)\vec{k}+sin\theta\cdot(\vec k\times\vec v) \end{aligned}
vrot=v∣∣+v⊥rot=(v⋅k)k+cosθ⋅v⊥+sinθ⋅(k×v⊥)=(v⋅k)k+cosθ⋅(v−(v⋅k)k)+sinθ⋅(k×(v−v∣∣))=cosθ⋅v+(1−cosθ)⋅(v⋅k)k+sinθ⋅(k×v)
得证。
把
v
v
v提取出来,写成矩阵形式:
v
r
o
t
=
c
o
s
θ
⋅
v
+
(
1
−
c
o
s
θ
)
⋅
(
v
T
⋅
k
)
k
+
s
i
n
θ
⋅
(
k
×
v
)
=
R
⋅
v
\begin{aligned} v_{rot}&=cos\theta\cdot v+(1-cos\theta)\cdot(v^T \cdot k)k+sin\theta\cdot(k\times v)\\ &=R\cdot v \end{aligned}
vrot=cosθ⋅v+(1−cosθ)⋅(vT⋅k)k+sinθ⋅(k×v)=R⋅v
因为
(
v
T
⋅
k
)
k
=
k
(
v
T
⋅
k
)
=
k
(
k
T
⋅
v
)
=
k
⋅
k
T
⋅
v
(v^T \cdot k )k=k(v^T \cdot k)=k(k^T \cdot v)=k\cdot k^T \cdot v
(vT⋅k)k=k(vT⋅k)=k(kT⋅v)=k⋅kT⋅v
又因为
k
⃗
×
v
⃗
=
(
k
2
v
3
−
k
3
v
2
)
⋅
i
⃗
+
(
k
3
v
1
−
k
1
v
3
)
⋅
j
⃗
+
(
k
1
v
2
−
k
2
v
1
)
⋅
k
⃗
=
[
k
2
v
3
−
k
3
v
2
k
3
v
1
−
k
1
v
3
k
1
v
2
−
k
2
v
1
]
=
[
0
,
−
k
3
,
k
2
k
3
,
0
,
−
k
1
−
k
2
,
k
1
,
0
]
[
v
1
v
2
v
3
]
=
[
0
,
−
k
3
,
k
2
k
3
,
0
,
−
k
1
−
k
2
,
k
1
,
0
]
⋅
v
⃗
\begin{aligned} \vec k\times \vec v&=(k_2v_3-k_3v_2)\cdot \vec i+(k_3v_1-k_1v_3)\cdot \vec j+(k_1v_2-k_2v_1)\cdot \vec k\\ &=\left[\begin{array}{l} k_2v_3-k_3v_2\\ k_3v_1-k_1v_3\\ k_1v_2-k_2v_1 \end{array}\right]\\ &=\left[\begin{array}{l} 0,-k_3,k_2\\ k_3,0,-k_1\\ -k_2,k_1,0 \end{array}\right] \left[\begin{array}{l} v_1\\ v_2\\ v_3 \end{array}\right]=\left[\begin{array}{l} 0,-k_3,k_2\\ k_3,0,-k_1\\ -k_2,k_1,0 \end{array}\right]\cdot \vec v \end{aligned}
k×v=(k2v3−k3v2)⋅i+(k3v1−k1v3)⋅j+(k1v2−k2v1)⋅k=⎣
⎡k2v3−k3v2k3v1−k1v3k1v2−k2v1⎦
⎤=⎣
⎡0,−k3,k2k3,0,−k1−k2,k1,0⎦
⎤⎣
⎡v1v2v3⎦
⎤=⎣
⎡0,−k3,k2k3,0,−k1−k2,k1,0⎦
⎤⋅v
即:
R
=
c
o
s
θ
⋅
I
3
×
3
+
(
1
−
c
o
s
θ
)
⋅
(
k
⃗
⋅
k
⃗
T
)
+
s
i
n
θ
⋅
[
0
,
−
k
3
,
k
2
k
3
,
0
,
−
k
1
−
k
2
,
k
1
,
0
]
R=cos\theta\cdot I_{3\times3}+(1-cos\theta)\cdot(\vec k \cdot \vec k^T) +sin\theta\cdot\left[\begin{array}{l} 0,-k_3,k_2\\ k_3,0,-k_1\\ -k_2,k_1,0 \end{array}\right]
R=cosθ⋅I3×3+(1−cosθ)⋅(k⋅kT)+sinθ⋅⎣
⎡0,−k3,k2k3,0,−k1−k2,k1,0⎦
⎤
罗德里格旋转公式给出了轴角->旋转矩阵的转换:
R
=
c
o
s
θ
⋅
I
3
×
3
+
(
1
−
c
o
s
θ
)
⋅
(
k
⃗
⋅
k
⃗
T
)
+
s
i
n
θ
⋅
[
0
,
−
k
3
,
k
2
k
3
,
0
,
−
k
1
−
k
2
,
k
1
,
0
]
R=cos\theta\cdot I_{3\times3}+(1-cos\theta)\cdot(\vec k \cdot \vec k^T) +sin\theta\cdot\left[\begin{array}{l} 0,-k_3,k_2\\ k_3,0,-k_1\\ -k_2,k_1,0 \end{array}\right]
R=cosθ⋅I3×3+(1−cosθ)⋅(k⋅kT)+sinθ⋅⎣
⎡0,−k3,k2k3,0,−k1−k2,k1,0⎦
⎤