罗德里格斯公式及其推导

文章目录

罗德里格斯公式及其推导
一、基础准备
- 1. 旋转矩阵
- 2. 旋转向量
- 3. 向量叉积及反对称矩阵
二、公式推导
- 1. 符号说明与图例
- 2. 推导
三、公式应用及深入理解
四、极限方式推导

罗格里格斯公式，将3D旋转表达成了 ( n ∧ , θ ) (n^∧,θ) (n∧,θ)的形式，一般记作
ω = θ ∗ n = ( ω x , ω y , ω z ) T ω = θ*n=(ω_x,ω_y,ω_z)^T ω=θ∗n=(ωx,ωy,ωz)T
两种表达式
R = [ I + ( 1 − c o s ( θ ) ) ∗ N 2 + s i n ( θ ) ∗ N ] R =[I + (1 - cos(θ)) * N^2 + sin(θ) * N] R=[I+(1−cos(θ))∗N2+sin(θ)∗N]
R = c o s ( θ ) ∗ I + ( 1 − c o s ( θ ) ) ∗ n ∗ n T + s i n ( θ ) ∗ n ∧ R =cos(θ) * I + (1 - cos(θ)) * n * n^T + sin(θ) * 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{matrix} 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{matrix}\right] R=⎣ ⎡rxxryxrzxrxyryyrzyrxzryzrzz⎦ ⎤
R矩阵是标准正交矩阵，用于基向量之间的刚性变换，且满足：

R T R = R − 1 R = E R^TR = R^{-1}R = E RTR=R−1R=E
|R| = 1

2. 旋转向量

旋转过程用一个单位向量n和旋转角度θ来表示。设旋转向量为：
n = [ n 1 n 2 n 3 ] n= \left[\begin{matrix} n_1 \\ n_2 \\ n_3 \end{matrix}\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. 向量叉积及反对称矩阵

a × b = a ∗ b ∗ s i n ( θ ) a × b = a*b*sin(θ) a×b=a∗b∗sin(θ)
叉积为矢量，长度为两个向量相夹的面积，方向与两个向量垂直。如下图：

我们设置向量
a = [ a 1 a 2 a 3 ] a= \left[\begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix}\right] a=⎣ ⎡a1a2a3⎦ ⎤
a的反对称矩阵为：
a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] = A a^∧ = \left[\begin{matrix} 0 & -a_3 &a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0 \end{matrix}\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 × b= \left[\begin{matrix} a_2b_3 - a_3b_2 \\ -a_1b_3 + a_3b_1 \\ a_1b_2 - a_2b_1 \end{matrix}\right]=\left[\begin{matrix} 0 & -a_3 &a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0 \end{matrix}\right]*\left[\begin{matrix} b_1 \\ b_2 \\ b_3 \end{matrix}\right] = a^∧*b a×b=⎣ ⎡a2b3−a3b2−a1b3+a3b1a1b2−a2b1⎦ ⎤=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤∗⎣ ⎡b1b2b3⎦ ⎤=a∧∗b

二、公式推导

1. 符号说明与图例

名称	符号
旋转向量	n n n
反对称矩阵	n ∧ o r N n^∧ or N n∧orN
旋转角度	θ θ θ
旋转点	P P P
O P OP OP向量	v v v
v v v旋转 θ θ θ后的向量	v ′ v^{'} v′
v v v在旋转平面的投影	v ⊥ v_⊥ v⊥
v v v在旋转轴的投影	v ∥ v_∥ v∥
v v v旋转 θ θ θ后旋转平面的分量	v ⊥ ′ v^{'}_⊥ v⊥′
v v v旋转 θ θ θ后旋转轴 n n n的分量	v ∥ ′ v^{'}_∥ v∥′
w w w向量，垂直于 n n n与 v v v所在的平面，且与 n n n、 v v v均正交	w w w

2. 推导

假定空间中一个点 P P P，构成向量 v = O P → v=\overrightarrow{OP} v=OP &，对其进行分解：
v = v ⊥ + v ∥ v=v_⊥ + v_∥ v=v⊥+v∥
同理旋转后的向量：
v ′ = v ⊥ ′ + v ∥ ′ v^{'}=v^{'}_⊥ + v^{'}_∥ v′=v⊥′+v∥′
显然：
v ∥ = v ∥ ′ v_∥=v^{'}_∥ v∥=v∥′
所以有：
v ′ = v ⊥ ′ + v ∥ v^{'}=v^{'}_⊥ + v_∥ v′=v⊥′+v∥
又向量 w w w满足：
w = n × v w=n×v w=n×v
那么：
∣ w ∣ = ∣ n × v ∣ |w|=|n×v| ∣w∣=∣n×v∣
由于 ∣ w ∣ |w| ∣w∣代表黄色平行四边形阴影面积，而面积计算也可以使用 ∣ n ∣ ∗ ∣ v ⊥ ∣ |n|*|v_⊥| ∣n∣∗∣v⊥∣，而 n n n为单位向量，所以有
∣ w ∣ = ∣ n × v ∣ = ∣ v ⊥ ∣ |w|=|n×v|=|v_⊥| ∣w∣=∣n×v∣=∣v⊥∣
由于 n × w n×w n×w与 v ⊥ v_⊥ v⊥相反，我们可以用 n n n和 v v v表示出 v ⊥ v_⊥ v⊥和 v ∥ v_∥ v∥，即：
v ⊥ = − n × w = − n × ( n × v ) v_⊥=-n×w=-n×(n×v) v⊥=−n×w=−n×(n×v)
v ∥ = v − v ⊥ = v + n × ( n × v ) v_∥=v - v_⊥=v+n×(n×v) v∥=v−v⊥=v+n×(n×v)
由于 v ⊥ v_⊥ v⊥与向量 w w w合成即为 v ⊥ ′ v^{'}_⊥ v⊥′，所以有：
v ⊥ ′ = v ⊥ + w = c o s ( θ ) ∗ v ⊥ + s i n ( θ ) ∗ w v^{'}_⊥ = v_⊥ + w = cos(θ)*v_⊥ + sin(θ) * w v⊥′=v⊥+w=cos(θ)∗v⊥+sin(θ)∗w
将上式带入有：
v ′ = v ∥ + v ⊥ ′ = ( v − v ⊥ ) + c o s ( θ ) ∗ v ⊥ + s i n ( θ ) ∗ w = v + ( c o s ( θ ) − 1 ) ∗ v ⊥ + s i n ( θ ) ∗ w = v + ( 1 − c o s ( θ ) ) ∗ n ∧ ∗ ( n ∧ ∗ v ) + s i n ( θ ) ∗ n ∧ ∗ v = v + ( 1 − c o s ( θ ) ) ∗ N ∗ N ∗ v + s i n ( θ ) ∗ N ∗ v = [ I + ( 1 − c o s ( θ ) ) ∗ N 2 + s i n ( θ ) ∗ N ] ∗ v \begin{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 \\ & = [I + (1- cos(θ))*N^2 +sin(θ) *N ]*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=[I+(1−cos(θ))∗N2+sin(θ)∗N]∗v
所以有：
R = [ I + ( 1 − c o s ( θ ) ) ∗ N 2 + s i n ( θ ) ∗ N ] R=[I + (1- cos(θ))*N^2 +sin(θ) *N ] R=[I+(1−cos(θ))∗N2+sin(θ)∗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 1 2 n 3 n 3 2 ] n*n^T = \left[\begin{matrix} n_1^2 & n_1n_2 & n_1n_3\\ n_1n_2 & n_2^2 & n_2n_3\\ n_1n_3 & n_12n_3 & n_3^2 \end{matrix}\right] n∗nT=⎣ ⎡n12n1n2n1n3n1n2n22n12n3n1n3n2n3n32⎦ ⎤
n ∧ = [ 0 − n 3 n 2 n 3 0 − n 1 − n 2 n 1 0 ] = N n^∧ = \left[\begin{matrix} 0 & -n_3 &n_2\\ n_3 & 0 & -n_1\\ -n_2 & n_1 & 0 \end{matrix}\right] = N n∧=⎣ ⎡0n3−n2−n30n1n2−n10⎦ ⎤=N
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 1 2 n 3 − n 1 2 − n 2 2 ] N*N = \left[\begin{matrix} -n_2^2-n_3^2 & n_1n_2 & n_1n_3\\ n_1n_2 & -n_1^2-n_3^2 & n_2n_3\\ n_1n_3 & n_12n_3 & -n_1^2-n_2^2 \end{matrix}\right] N∗N=⎣ ⎡−n22−n32n1n2n1n3n1n2−n12−n32n12n3n1n3n2n3−n12−n22⎦ ⎤
n ∗ n T = I + N ∗ N n*n^T = I + N*N n∗nT=I+N∗N
将上式带入表达式一，则有：
R = c o s ( θ ) ∗ I + ( 1 − c o s ( θ ) ) ∗ n ∗ n T + s i n ( θ ) ∗ n ∧ R =cos(θ) * I + (1 - cos(θ)) * n * n^T + sin(θ) * n^∧ R=cos(θ)∗I+(1−cos(θ))∗n∗nT+sin(θ)∗n∧

三、公式应用及深入理解

罗德里格斯旋转公式用于旋转三维空间的向量.它需要知道旋转轴和旋转角度。
罗德里格斯旋转公式，将3D旋转表示成绕空间中某一旋转轴 n n n旋转角度 θ θ θ的形式，一般记作：
q = θ ∗ n = [ q x q y q z ] q=θ*n= \left[\begin{matrix} q_x \\ q_y \\ q_z \end{matrix}\right] q=θ∗n=⎣ ⎡qxqyqz⎦ ⎤
式中， θ = ∣ q ∣ = q x 2 + q y 2 + q z 2 θ=|q|=\sqrt{q_x^2+q_y^2+q_z^2} θ=∣q∣=qx2+qy2+qz2 。
这种表示形式非常简洁，但存在奇异问题，主要在于：

旋转角度为 θ θ θ和 θ + 2 k π θ+2kπ θ+2kπ的结果是一样的；
( n , θ ) (n,θ) (n,θ)和 ( − n , − θ ) (-n,-θ) (−n,−θ)的结果是一样的；

对于非常小的旋转，旋转矩阵 R R R和罗德里格斯向量 q q q存在线性关系，推导如下：
R ( q ) = R ( n , θ ) = I + ( 1 − c o s ( θ ) ) ∗ N 2 + s i n ( θ ) ∗ N ≈ I + s i n ( θ ) ∗ N ≈ I + θ ∗ N = I + q ∧ = [ 1 − q z q y q z 1 − q x − q y q x 1 ] \begin{aligned} \ R(q) & = R(n,θ)\\ & = I + (1- cos(θ))*N^2 +sin(θ) *N \\ & ≈ I + sin(θ) *N \\ & ≈ I +θ *N\\ & = I +q^∧ \\ & = \left[\begin{matrix} 1 & -q_z &q_y\\ q_z & 1 & -q_x\\ -q_y & q_x & 1 \end{matrix}\right] \end{aligned} R(q)=R(n,θ)=I+(1−cos(θ))∗N2+sin(θ)∗N≈I+sin(θ)∗N≈I+θ∗N=I+q∧=⎣ ⎡1qz−qy−qz1qxqy−qx1⎦ ⎤

四、极限方式推导

一次性绕旋转轴 n n n旋转角度 θ θ θ，等价于绕旋转轴 n n n旋转 k k k次，每次旋转角度为 θ / k θ/k θ/k。
因此：
R ( n , θ ) = lim ⁡ k → + ∞ ( I + 1 k ∗ θ ∗ N ) k = e θ ∗ N R(n,θ) = \lim_{k\rightarrow+\infty}(I + \frac{1}{k}*θ*N)^k=e^{θ*N} R(n,θ)=k→+∞lim(I+k1∗θ∗N)k=eθ∗N
而：
e θ ∗ N = I + ( θ ∗ N ) + ( θ ∗ N ) 2 2 ! + ( θ ∗ N ) 3 3 ! + ⋯ e^{θ*N}=I + (θ*N) + \frac{(θ*N)^2}{2!}+ \frac{(θ*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 ! − ( θ ) 4 4 ! + ⋯ ） ∗ N 2 = I + s i n ( θ ) ∗ N + ( 1 − c o s ( θ ) ) ∗ N 2 ] \begin{aligned}e^{θ*N} &= I + (θ - \frac{(θ)^3}{3!} + ⋯）* N + (\frac{(θ)^2}{2!} - \frac{(θ)^4}{4!} + ⋯）* N^2 \\ &= I + sin(θ) * N + (1 - cos(θ)) * N^2] \end{aligned} eθ∗N=I+(θ−3!(θ)3+⋯）∗N+(2!(θ)2−4!(θ)4+⋯）∗N2=I+sin(θ)∗N+(1−cos(θ))∗N2]