罗德里格斯公式推导
一，基础准备1. 旋转矩阵2. 旋转向量3. 向量叉积
二，公式推导1.符号说明与图例2. 公式推导
三，公式理解和深入四，极限的方式简洁推导罗德里格斯公式
罗德里格斯公式，将3D旋转表达成了
(
n
∧
,
θ
)
(n^{\wedge}, \theta)
(n∧,θ)的形式，一般记作
ω
=
θ
∗
n
=
(
ω
x
,
ω
y
,
ω
z
)
T
\omega = \theta * n = \left(\omega_{x}, \omega_{y}, \omega_{z}\right)^{T}
ω=θ∗n=(ωx,ωy,ωz)T 两种表达：
R
=
[
I
+
(
1
−
cos
(
θ
)
)
∗
N
2
+
sin
(
θ
)
∗
N
]
R = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N]
R=[I+(1−cos(θ))∗N2+sin(θ)∗N]
R
=
cos
(
θ
)
∗
I
+
(
1
−
cos
(
θ
)
)
∗
n
∗
n
T
+
sin
(
θ
)
∗
n
R = \cos (\theta) * I+(1-\cos (\theta)) * n * n^{T}+\sin (\theta) * n
R=cos(θ)∗I+(1−cos(θ))∗n∗nT+sin(θ)∗n
一，基础准备
1. 旋转矩阵
R
=
[
r
x
x
r
x
y
r
x
z
r
y
x
r
y
y
r
y
z
r
z
x
r
z
y
r
z
z
]
R=\left[\begin{array}{lll}{r_{x x}} & {r_{x y}} & {r_{x z}} \\ {r_{y x}} & {r_{y y}} & {r_{y z}} \\ {r_{z x}} & {r_{z y}} & {r_{z z}}\end{array}\right]
R=
rxxryxrzxrxyryyrzyrxzryzrzz
R矩阵是标准正交矩阵，用于基向量之间的刚性变换。满足
R
T
R
=
R
−
1
R
=
E
R^{T} R=R^{-1} R=E
RTR=R−1R=E
∣
R
∣
=
1
|R|=1
∣R∣=1
2. 旋转向量
用一个单位向量n和旋转角度
θ
\theta
θ来表示旋转的过程。默认向量是列向量
n
=
[
n
1
n
2
n
3
]
n=\left[\begin{array}{l}{n_{1}} \\ {n_{2}} \\ {n_{3}}\end{array}\right]
n=
n1n2n3
n
1
2
+
n
2
2
+
n
3
2
=
1
n_{1}^{2}+n_{2}^{2}+n_{3}^{2}=1
n12+n22+n32=1
3. 向量叉积
图片来源:https://www.mathsisfun.com/algebra/vectors-cross-product.html 两个向量的叉积可以写成一个反对称矩阵与一个向量的点积
a
=
[
a
1
a
2
a
3
]
a=\left[\begin{array}{l}{a_{1}} \\ {a_{2}} \\ {a_{3}}\end{array}\right]
a=
a1a2a3
a
∧
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
=
A
a^{\wedge}=\left[\begin{array}{ccc}{0} & {-a_{3}} & {a_{2}} \\ {a_{3}} & {0} & {-a_{1}} \\ {-a_{2}} & {a_{1}} & {0}\end{array}\right]=A
a∧=
0a3−a2−a30a1a2−a10
=A
a
×
b
=
[
a
2
b
3
−
a
3
b
2
−
a
1
b
3
+
a
3
b
1
a
1
b
2
−
a
2
b
1
]
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
∗
[
b
1
b
2
b
3
]
=
A
∗
b
a \times b=\left[\begin{array}{c}{a_{2} b_{3}-a_{3} b_{2}} \\ {-a_{1} b_{3}+a_{3} b_{1}} \\ {a_{1} b_{2}-a_{2} b_{1}}\end{array}\right]=\left[\begin{array}{ccc}{0} & {-a_{3}} & {a_{2}} \\ {a_{3}} & {0} & {-a_{1}} \\ {-a_{2}} & {a_{1}} & {0}\end{array}\right] *\left[\begin{array}{l}{b_{1}} \\ {b_{2}} \\ {b_{3}}\end{array}\right]=A * b
a×b=
a2b3−a3b2−a1b3+a3b1a1b2−a2b1
=
0a3−a2−a30a1a2−a10
∗
b1b2b3
=A∗b 所以两个向量叉积可以写成一个反对称矩阵与向量的点积 乘积
a
×
b
=
a
∧
∗
b
a \times b=a^{\wedge} * b
a×b=a∧∗b
二，公式推导
1.符号说明与图例
名称符号旋转向量
n
n
n旋转向量的反对称矩阵形式
n
∧
o
r
N
n^{\wedge} or N
n∧orN旋转角度
θ
\theta
θ旋转点POP向量
v
v
vv旋转
θ
\theta
θ后的向量
v
′
v^{'}
v′v在旋转平面的投影
v
⊥
v_{\perp}
v⊥v在旋转轴的投影
v
∥
v_{\parallel}
v∥v旋转
θ
\theta
θ后旋转平面的分量
v
⊥
′
v_{\perp}^{'}
v⊥′v旋转
θ
\theta
θ后在n的分量
v
∥
′
v_{\parallel}^{'}
v∥′w向量，垂直与n向量与v向量的平面, w与n向量与v向量都正交v方向的单位向量$\frac{v}{\leftv\right
2. 公式推导
空间中的一个点P，构成向量
v
=
O
P
→
v=\overrightarrow{O P}
v=OP
，则分解后有
v
=
v
⊥
+
v
∥
v = v_{\perp} + v_{\parallel}
v=v⊥+v∥ 同理旋转后的向量
v
′
=
v
⊥
′
+
v
∥
′
v' = v_{\perp}^{'} + v_{\parallel}^{'}
v′=v⊥′+v∥′ 显然
v
∥
=
v
∥
′
v_{\parallel} = v_{\parallel}^{'}
v∥=v∥′ 所以
v
′
=
v
⊥
′
+
v
∥
v^{'} = v_{\perp}^{'} + v_{\parallel}
v′=v⊥′+v∥ 定义一个向量w
w
=
n
×
v
w=n \times v
w=n×v 根据几何意义，w的模长就是黄色阴影的的面积。即
∣
w
∣
=
∣
n
×
v
∣
|w|=|n \times v|
∣w∣=∣n×v∣ 这部分面积也可以看成平行四边形面积公式底乘以高，因为n向量的模长是1，高就是
∣
v
⊥
∣
|v_{\perp}|
∣v⊥∣ 所以
∣
w
∣
=
∣
n
×
v
∣
=
∣
v
⊥
∣
|w|=|n \times v|=|v_{\perp}|
∣w∣=∣n×v∣=∣v⊥∣ 而由于
n
×
w
n \times w
n×w与
v
⊥
v_{\perp}
v⊥方向相反 至此我们可以用n与v向量表示出v的两个投影的分量，即
v
⊥
=
−
n
×
w
=
−
n
×
(
n
×
v
)
v_{\perp}=-n \times w=-n \times(n \times v)
v⊥=−n×w=−n×(n×v)
v
∥
=
v
−
v
⊥
=
v
+
n
×
(
n
×
v
)
v_{\parallel}=v-v_{\perp}=v+n \times(n \times v)
v∥=v−v⊥=v+n×(n×v) 我们现在讲v’表示出来
v
⊥
′
=
∣
v
⊥
∣
∗
[
cos
(
θ
)
∗
v
⊥
∣
v
⊥
∣
+
sin
(
θ
)
∗
w
∣
w
∣
]
=
cos
(
θ
)
∗
v
⊥
+
sin
(
θ
)
∗
∣
v
⊥
∣
∗
w
∣
w
∣
=
cos
(
θ
)
∗
v
⊥
+
sin
(
θ
)
∗
w
\begin{aligned} v_{\perp}^{\prime} &=\left|v_{\perp}\right| *\left[\cos (\theta) * \frac{v_{\perp}}{\left|v_{\perp}\right|}+\sin (\theta) * \frac{w}{|w|}\right] \\ &=\cos (\theta) * v_{\perp}+\sin (\theta) *\left|v_{\perp}\right| * \frac{w}{|w|} \\ &=\cos (\theta) * v_{\perp}+\sin (\theta) * w \end{aligned}
v⊥′=∣v⊥∣∗[cos(θ)∗∣v⊥∣v⊥+sin(θ)∗∣w∣w]=cos(θ)∗v⊥+sin(θ)∗∣v⊥∣∗∣w∣w=cos(θ)∗v⊥+sin(θ)∗w 即
v
⊥
′
=
cos
(
θ
)
∗
v
⊥
+
sin
(
θ
)
∗
w
\begin{aligned} v_{\perp}^{\prime} = \cos (\theta) * v_{\perp}+\sin (\theta) * w \end{aligned}
v⊥′=cos(θ)∗v⊥+sin(θ)∗w 将上式子带入
v
′
=
v
/
/
+
v
⊥
′
=
(
v
−
v
⊥
)
+
cos
(
θ
)
∗
v
⊥
+
sin
(
θ
)
∗
w
=
v
+
(
cos
(
θ
)
−
1
)
∗
v
⊥
+
sin
(
θ
)
∗
w
=
v
+
(
1
−
cos
(
θ
)
)
∗
n
×
(
n
×
v
)
+
sin
(
θ
)
∗
n
×
v
=
v
+
(
1
−
cos
(
θ
)
)
∗
n
∧
∗
(
n
∧
∗
v
)
+
sin
(
θ
)
∗
n
∧
∗
v
=
v
+
(
1
−
cos
(
θ
)
)
∗
N
∗
N
∗
v
+
sin
(
θ
)
∗
N
∗
v
=
v
+
(
1
−
cos
(
θ
)
)
∗
N
2
∗
v
+
sin
(
θ
)
∗
N
∗
v
=
[
I
+
(
1
−
cos
(
θ
)
)
∗
N
2
+
sin
(
θ
)
∗
N
]
∗
v
\begin{aligned} v^{\prime} &=v_{/ /}+v_{\perp}^{\prime} \\ &=\left(v-v_{\perp}\right)+\cos (\theta) * v_{\perp}+\sin (\theta) * w \\ &=v+(\cos (\theta)-1) * v_{\perp}+\sin (\theta) * w \\ &=v+(1-\cos (\theta)) * n \times(n \times v)+\sin (\theta) * n \times v \\ &=v+(1-\cos (\theta)) * n^{\wedge} * (n^{\wedge} * v)+\sin (\theta) * n^{\wedge} * v \\ &=v+(1-\cos (\theta)) * N * N * v+\sin (\theta) * N * v \\ &=v+(1-\cos (\theta)) * N^{2} * v+\sin (\theta) * N * v \\ &=\left[I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N\right] * v \end{aligned}
v′=v//+v⊥′=(v−v⊥)+cos(θ)∗v⊥+sin(θ)∗w=v+(cos(θ)−1)∗v⊥+sin(θ)∗w=v+(1−cos(θ))∗n×(n×v)+sin(θ)∗n×v=v+(1−cos(θ))∗n∧∗(n∧∗v)+sin(θ)∗n∧∗v=v+(1−cos(θ))∗N∗N∗v+sin(θ)∗N∗v=v+(1−cos(θ))∗N2∗v+sin(θ)∗N∗v=[I+(1−cos(θ))∗N2+sin(θ)∗N]∗v
至此我们就可以得到：
v
′
=
[
I
+
(
1
−
cos
(
θ
)
)
∗
N
2
+
sin
(
θ
)
∗
N
]
∗
v
v^{'} = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N]* v
v′=[I+(1−cos(θ))∗N2+sin(θ)∗N]∗v 所以
R
=
[
I
+
(
1
−
cos
(
θ
)
)
∗
N
2
+
sin
(
θ
)
∗
N
]
R = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N]
R=[I+(1−cos(θ))∗N2+sin(θ)∗N] 有些地方习惯用n来表示，那么继续推导
n
∗
n
T
=
[
n
1
2
n
1
n
2
n
1
n
3
n
1
n
2
n
2
2
n
2
n
3
n
1
n
3
n
2
n
3
n
3
2
]
n * n^{T}=\left[\begin{array}{ccc}{n_{1}^{2}} & {n_{1} n_{2}} & {n_{1} n_{3}} \\ {n_{1} n_{2}} & {n_{2}^{2}} & {n_{2} n_{3}} \\ {n_{1} n_{3}} & {n_{2} n_{3}} & {n_{3}^{2}}\end{array}\right]
n∗nT=
n12n1n2n1n3n1n2n22n2n3n1n3n2n3n32
n
∧
=
N
=
[
0
−
n
3
n
2
n
3
0
−
n
1
−
n
2
n
1
0
]
n^{\wedge}=N=\left[\begin{array}{ccc}{0} & {-n_{3}} & {n_{2}} \\ {n_{3}} & {0} & {-n_{1}} \\ {-n_{2}} & {n_{1}} & {0}\end{array}\right]
n∧=N=
0n3−n2−n30n1n2−n10
N
∗
N
=
[
−
n
2
2
−
n
3
2
n
1
n
2
n
1
n
3
n
1
n
2
−
n
1
2
−
n
3
2
n
2
n
3
n
1
n
3
n
2
n
3
−
n
1
2
−
n
2
2
]
N * N=\left[\begin{array}{ccc}{-n_{2}^{2}-n_{3}^{2}} & {n_{1} n_{2}} & {n_{1} n_{3}} \\ {n_{1} n_{2}} & {-n_{1}^{2}-n_{3}^{2}} & {n_{2} n_{3}} \\ {n_{1} n_{3}} & {n_{2} n_{3}} & {-n_{1}^{2}-n_{2}^{2}}\end{array}\right]
N∗N=
−n22−n32n1n2n1n3n1n2−n12−n32n2n3n1n3n2n3−n12−n22
n
∗
n
T
=
I
+
N
∗
N
n * n^{T}=I+N * N
n∗nT=I+N∗N
N
∗
N
=
n
∗
n
T
−
I
N * N=n * n^{T}-I
N∗N=n∗nT−I 带入
R
=
[
I
+
(
1
−
cos
(
θ
)
)
∗
N
2
+
sin
(
θ
)
∗
N
]
R = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N]
R=[I+(1−cos(θ))∗N2+sin(θ)∗N]可以得到
R
=
cos
(
θ
)
∗
I
+
(
1
−
cos
(
θ
)
)
∗
n
∗
n
T
+
sin
(
θ
)
∗
n
R = \cos (\theta) * I+(1-\cos (\theta)) * n * n^{T}+\sin (\theta) * n
R=cos(θ)∗I+(1−cos(θ))∗n∗nT+sin(θ)∗n
三，公式理解和深入
罗德里格斯公式，将3D旋转表达成了
(
n
∧
,
θ
)
(n^{\wedge}, \theta)
(n∧,θ)的形式，一般记作
ω
=
θ
∗
n
=
(
ω
x
,
ω
y
,
ω
z
)
T
\omega = \theta * n = \left(\omega_{x}, \omega_{y}, \omega_{z}\right)^{T}
ω=θ∗n=(ωx,ωy,ωz)T
这是一种非常简洁和简单的表示形式，但是这不是一个奇异的变换。主要原因有两点
旋转
θ
\theta
θ和旋转
θ
+
2
π
\theta + 2\pi
θ+2π表达了相同的旋转
(
n
∧
,
θ
)
(n^{\wedge}, \theta)
(n∧,θ)与
(
−
n
∧
,
−
θ
)
(-n^{\wedge}, -\theta)
(−n∧,−θ)也表达了相同的旋转 但是这样的好处在于，对于非常小的旋转，罗德里格斯公式可以表示成
R
(
ω
)
=
R
(
n
∧
,
θ
)
≈
I
+
s
i
n
(
θ
)
∗
N
≈
I
+
θ
∗
N
=
[
1
−
θ
∗
n
3
θ
∗
n
2
θ
∗
n
3
1
−
θ
∗
n
1
−
θ
∗
n
2
θ
∗
n
1
1
]
=
[
1
−
ω
z
ω
y
ω
z
1
ω
x
−
ω
y
ω
x
1
]
R(\omega) = R(n^{\wedge}, \theta) \approx I + sin(\theta) * N \\ \approx I + \theta * N \\=\left[\begin{array}{ccc}{1} & {-\theta * n_{3}} & {\theta * n_{2}} \\ {\theta *n_{3}} & {1} & {-\theta * n_{1}} \\ {-\theta * n_{2}} & {\theta *n_{1}} & {1}\end{array}\right] \\\\ = \left[\begin{array}{ccc}{1} & {-\omega_{z} } & {\omega_{y}} \\ {\omega_{z} } & {1} & {\omega_{x} } \\ {-\omega_{y} } & {\omega_{x} } & {1}\end{array}\right]
R(ω)=R(n∧,θ)≈I+sin(θ)∗N≈I+θ∗N=
1θ∗n3−θ∗n2−θ∗n31θ∗n1θ∗n2−θ∗n11
=
1ωz−ωy−ωz1ωxωyωx1
这个式子给出了，非常漂亮的的
ω
\omega
ω与R之间的线性的关系。 我们也可以写成，
R
v
≈
v
+
w
×
v
Rv \approx v + w \times v
Rv≈v+w×v,这样我们只需要简单的求导，就可以得到v
∂
R
v
∂
ω
=
−
v
=
[
0
z
−
y
−
z
0
x
y
−
x
0
]
\frac{\partial \boldsymbol{R} \boldsymbol{v}}{\partial \boldsymbol{\omega}}=-{v}=\left[\begin{array}{ccc}{0} & {z} & {-y} \\ {-z} & {0} & {x} \\ {y} & {-x} & {0}\end{array}\right]
∂ω∂Rv=−v=
0−zyz0−x−yx0
四，极限的方式简洁推导罗德里格斯公式
叫做exponential twist(Murray, Li, and Sastry 1994), 旋转
θ
\theta
θ角度，等价于旋转k次
θ
/
k
\theta/k
θ/k角度。
R
(
n
∧
,
θ
)
=
lim
k
→
∞
(
I
+
1
k
(
θ
∗
Ｎ
)
)
k
=
e
θ
∗
N
R(n^{\wedge}, \theta) = \lim _{k \rightarrow \infty}\left(\boldsymbol{I}+\frac{1}{k}(\theta *Ｎ)\right)^{k} = e^{\theta *N}
R(n∧,θ)=k→∞lim(I+k1(θ∗Ｎ))k=eθ∗N
e
θ
∗
N
=
I
+
(
θ
∗
N
)
+
(
θ
∗
N
)
2
2
+
(
θ
∗
N
)
3
3
!
+
.
.
.
e^{\theta *N} = I + (\theta *N) + \frac{(\theta *N)^{2}}{2} + \frac{(\theta *N)^{3}}{3!} + ...
eθ∗N=I+(θ∗N)+2(θ∗N)2+3!(θ∗N)3+... 因为
N
k
+
2
=
−
N
k
,
k
>
0
N^{k+2} = -N^{k}, k>0
Nk+2=−Nk,k>0 所以
e
θ
∗
N
=
I
+
(
θ
−
θ
3
3
!
+
.
.
.
)
∗
N
+
(
θ
2
2
−
θ
3
4
!
+
.
.
.
)
∗
N
2
=
I
+
s
i
n
θ
∗
N
+
(
1
−
c
o
s
θ
)
∗
N
2
e^{\theta *N} = I + (\theta - \frac{\theta^{3}}{3!}+...)*N + (\frac{\theta^{2}}{2} - \frac{\theta^{3}}{4!}+...)*N^{2} \\ = I+sin{\theta}*N + (1-cos{\theta})*N^{2}
eθ∗N=I+(θ−3!θ3+...)∗N+(2θ2−4!θ3+...)∗N2=I+sinθ∗N+(1−cosθ)∗N2