罗德里格斯(Rodrigues)旋转公式推导

转载:原文链接

1 罗德里格斯(Rodrigues)旋转公式简介

对于三维空间向量 v v v的旋转问题,给定罗德里格斯旋转向量 q q q(由旋转轴 n n n和旋转角度 θ \theta θ构成),那么,用罗德里格斯(Rodrigues)旋转公式就可以得出旋转后的向量 v ′ v' v,如下:

v ′ = v + ( 1 − c o s θ ) ∗ N 2 ⋅ v + s i n θ ∗ N ⋅ v v'=v+(1-cos\theta)*N^{2} \cdot v+sin\theta*N\cdot v v=v+(1cosθ)N2v+sinθNv (1)


v ′ = c o s θ ∗ v + ( 1 − c o s θ ) ∗ n ⋅ n T ⋅ v + s i n θ ∗ n ∧ ⋅ v v'=cos\theta*v+(1-cos\theta)*n\cdot n^{T}\cdot v+sin\theta*n^{\wedge}\cdot v v=cosθv+(1cosθ)nnTv+sinθnv (2)

式中:
v = [ v x v y v z ] v= \begin{bmatrix} v_{x} \\ v_{y} \\ v_{z} \\ \end{bmatrix} v= vxvyvz v ′ = [ v x ′ v y ′ v z ′ ] v'= \begin{bmatrix} v'_{x} \\ v'_{y} \\ v'_{z} \\ \end{bmatrix} v= vxvyvz n = [ n x n y n z ] n= \begin{bmatrix} n_{x} \\ n_{y} \\ n_{z} \\ \end{bmatrix} n= nxnynz n ∧ = N = [ 0 − n z n y n z 0 − n x − n y n x 0 ] n^{\wedge}=N= \begin{bmatrix} 0 &-n_{z} &n_{y} \\ n_{z} &0 &-n_{x}\\ -n_{y} &n_{x} &0\\ \end{bmatrix} n=N= 0nznynz0nxnynx0 (3)

n ∧ n^{\wedge} n N N N表示向量 n n n的反对称矩阵形式。

罗德里格斯旋转公式可以用旋转矩阵表示,即将罗德里格斯旋转向量 q q q转换成3*3的旋转矩阵 R R R表示,如下:

v ′ = [ I + ( 1 − c o s θ ) ∗ N 2 + s i n θ ∗ N ] ⋅ v = R ⋅ v v'=[I+(1-cos\theta)*N^{2} +sin\theta*N]\cdot v = R\cdot v v=[I+(1cosθ)N2+sinθN]v=Rv (4)


v ′ = [ c o s θ ∗ I + ( 1 − c o s θ ) ∗ n ⋅ n T + s i n θ ∗ n ∧ ] ⋅ v = R ⋅ v v'=[cos\theta*I+(1-cos\theta)*n\cdot n^{T}+sin\theta*n^{\wedge}]\cdot v =R\cdot v v=[cosθI+(1cosθ)nnT+sinθn]v=Rv (5)

因此,罗德里格斯向量将3D旋转表示成 ( n , θ ) (n,\theta) (n,θ)的形式,一般记作:
q = θ ∗ n = [ q x q y q z ] q=\theta*n=\begin{bmatrix} q_{x} \\ q_{y} \\ q_{z} \\ \end{bmatrix} q=θn= qxqyqz (6)

式中, θ = ∣ q ∣ = q x 2 + q y 2 + q z 2 \theta=|q|=\sqrt{q^2_x+q^2_y+q^2_z} θ=q=qx2+qy2+qz2
罗德里格斯旋转被广泛应用于空间解析几何和计算机图形学领域,已成为刚体旋转运动的基本计算公式之一。

2 基础准备

2.1 旋转矩阵

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 (7)

旋转矩阵是单位正交矩阵,用于基向量之间的刚性变换(旋转部分),因此满足如下特性:

  1. R ⋅ R T = R ⋅ R − 1 = I R\cdot R^{T}=R\cdot R^{-1}=I RRT=RR1=I
  2. r i ⋅ r j = 0    ( i , j = 1 , 2 , 3 , i ≠ j ) r_i\cdot r_j=0\;(i,j=1,2,3,i\neq j) rirj=0(i,j=1,2,3,i=j)
  3. r i ⋅ r i = ∣ r i ∣ = 1    ( i = 1 , 2 , 3 ) r_i\cdot r_i=|r_i|=1\;(i=1,2,3) riri=ri=1(i=1,2,3)
  4. ∣ R ∣ = 1 |R|=1 R=1;

2.2 旋转向量

一般旋转向量是用一个单位向量 n n n和旋转角度 θ \theta θ来表示旋转过程,向量默认是列向量。
n = [ n x n y n z ] n= \begin{bmatrix} n_{x} \\ n_{y} \\ n_{z} \\ \end{bmatrix} n= nxnynz ∣ n ∣ = n x 2 + n y 2 + n z 2 = 1 |n|=\sqrt{n^2_x+n^2_y+n^2_z}=1 n=nx2+ny2+nz2 =1
罗德里格斯旋转向量与一般旋转向量的异同点:

  • 相同点:向量方向相同;
  • 不同点:模长不同,一般旋转向量是单位向量,而罗德里格斯向量的模长是旋转角度。

2.3 向量叉积(Cross Product)

image.png

图1 向量叉积

向量 a a a与向量 b b b的叉积 a × b a\times b a×b,可以表示成向量 a a a的反对称矩阵 A A A与向量 b b b的点积,如下:

[ a x a y a z ] × [ b x b y b z ] = [ 0 − a z a y a z 0 − a x − a y a x 0 ] ⋅ [ b x b y b z ] \begin{bmatrix} a_{x} \\ a_{y} \\ a_{z} \\ \end{bmatrix}\times \begin{bmatrix} b_{x} \\ b_{y} \\ b_{z} \\ \end{bmatrix}= \begin{bmatrix} 0 &-a_{z} &a_{y} \\ a_{z} &0 &-a_{x}\\ -a_{y} &a_{x} &0\\ \end{bmatrix}\cdot \begin{bmatrix} b_{x} \\ b_{y} \\ b_{z} \\ \end{bmatrix} axayaz × bxbybz = 0azayaz0axayax0 bxbybz (8)

上式可被简写成:

a × b = a ∧ ⋅ b = A ⋅ b a\times b=a^{\wedge}\cdot b=A\cdot b a×b=ab=Ab (9)

两向量叉积后所得向量的物理意义:其模长表示两向量构成平行四边形的面积。

另外补充:

a ⋅ a T = [ a x a y a z ] ⋅ [ a x a y a z ] = [ a x 2 a x a y a x a z a x a y a y 2 a y a z a x a z a y a z a z 2 ] a\cdot a^{T}=\begin{bmatrix} a_{x} \\ a_{y} \\ a_{z} \\ \end{bmatrix}\cdot \begin{bmatrix} a_{x} &a_{y} &a_{z} \end{bmatrix}= \begin{bmatrix} a_x^2 &a_x a_y &a_x a_z \\ a_x a_y &a_y^2 &a_y a_z\\ a_x a_z &a_y a_z &a_z^2\\ \end{bmatrix} aaT= axayaz [axayaz]= ax2axayaxazaxayay2ayazaxazayazaz2 (10)

A ⋅ A = [ 0 − a z a y a z 0 − a x − a y a x 0 ] ⋅ [ 0 − a z a y a z 0 − a x − a y a x 0 ] = [ − a y 2 − a z 2 a x a y a x a z a x a y − a x 2 − a z 2 a y a z a x a z a y a z − a x 2 − a y 2 ] A\cdot A=\begin{bmatrix} 0 &-a_{z} &a_{y} \\ a_{z} &0 &-a_{x}\\ -a_{y} &a_{x} &0\\ \end{bmatrix}\cdot \begin{bmatrix} 0 &-a_{z} &a_{y} \\ a_{z} &0 &-a_{x}\\ -a_{y} &a_{x} &0\\ \end{bmatrix}= \begin{bmatrix} -a_y^2-a_z^2 &a_x a_y &a_x a_z \\ a_x a_y &-a_x^2-a_z^2 &a_y a_z\\ a_x a_z &a_y a_z &-a_x^2-a_y^2\\ \end{bmatrix} AA= 0azayaz0axayax0 0azayaz0axayax0 = ay2az2axayaxazaxayax2az2ayazaxazayazax2ay2 (11)

因此,对于单位向量,满足:
a ⋅ a T = I + A ⋅ A a\cdot a^{T}=I+A\cdot A aaT=I+AA (12)

3 罗德里格斯旋转公式推导

3.1 符号说明

image.png

图2 向量旋转示意图

对图2中的相关符号作如下说明:
image.png

注:旋转平面是与旋转轴相垂直的平面。

3.2 公式推导

对于三维空间中的一点 P P P,构成向量 v = O P → v=\overrightarrow{OP} v=OP ,分解到旋转平面和旋转轴,如下:

v = v ⊥ + v ∥ v=v_{\perp}+v_{\parallel} v=v+v (13)

同理,对于旋转后的向量 v ′ v' v
v ′ = v ⊥ ′ + v ∥ ′ v'=v_{\perp}'+v_{\parallel}' v=v+v (14)

显然:
v ∥ ′ = v ∥ v_{\parallel}'=v_{\parallel} v=v (15)

因此:
v ′ = v ⊥ ′ + v ∥ v'=v_{\perp}'+v_{\parallel} v=v+v (16)

定义向量 w w w,如下:
w = n × v w=n\times v w=n×v (17)

根据几何意义, ∣ w ∣ |w| w为黄色阴影(平行四边形)的面积,这部分面积可等效成平行四边形底 n n n和高 v ⊥ v_{\perp} v得到的面积,因此:
w = n × v = n × v ⊥ w=n\times v = n\times v_{\perp} w=n×v=n×v (18)

且:
∣ w ∣ = ∣ v ⊥ ∣ = ∣ v ⊥ ′ ∣ |w|=|v_{\perp}|=|v_{\perp}'| w=v=v (19)

进而得到:
v ⊥ = − n × w = − n × ( n × v ) v_{\perp}=-n\times w=-n\times (n\times v) v=n×w=n×(n×v) (20)

以及:
v ∥ = v ∥ ′ = v − v ⊥ = v + n × ( n × v ) v_{\parallel}=v_{\parallel}'=v-v_{\perp}= v+n\times (n\times v) v=v=vv=v+n×(n×v) (21)

另外,将 v ⊥ ′ v_{\perp}' v v ⊥ v_{\perp} v w w w表示,如下:

v ⊥ ′ = ∣ v ⊥ ′ ∣ ∗ c o s θ ∗ v ⊥ ∣ v ⊥ ∣ + ∣ v ⊥ ′ ∣ ∗ s i n θ ∗ w ∣ w ∣ = c o s θ ∗ v ⊥ + s i n θ ∗ w v_{\perp}'= |v_{\perp}'|*cos\theta* \frac{v_{\perp}}{|v_{\perp}|}+ |v_{\perp}^{'}|*sin\theta* \frac{w}{|w|}= cos\theta*v_{\perp}+sin\theta*w v=vcosθvv+vsinθww=cosθv+sinθw (22)

将式(20)-(22)代入到式(16)中,最终可得:
v ′ = v ⊥ ′ + v ∥ = c o s θ ∗ v ⊥ + s i n θ ∗ w + v − v ⊥ = ( 1 − c o s θ ) ∗ n × ( n × v ) + s i n θ ∗ n × v + v = ( 1 − c o s θ ) ∗ N 2 ⋅ v + s i n θ ∗ N ⋅ v + v = [ ( 1 − c o s θ ) ∗ N 2 + s i n θ ∗ N + I ] ⋅ v = R ⋅ v \begin{aligned} &v^{'}=v_{\perp}^{'}+v_{\parallel}\\ & =cos\theta*v_{\perp}+sin\theta*w+ v-v_{\perp}\\ & =(1-cos\theta)*n\times (n\times v)+ sin\theta * n\times v+v\\ & =(1-cos\theta)*N^2\cdot v+ sin\theta * N\cdot v+v\\ & =[(1-cos\theta)*N^2+ sin\theta * N+I]\cdot v\\ & =R\cdot v \end{aligned} v=v+v=cosθv+sinθw+vv=(1cosθ)n×(n×v)+sinθn×v+v=(1cosθ)N2v+sinθNv+v=[(1cosθ)N2+sinθN+I]v=Rv (23)

因此:
R = I + ( 1 − c o s θ ) ∗ N 2 + s i n θ ∗ N R=I+(1-cos\theta)*N^2+ sin\theta * N R=I+(1cosθ)N2+sinθN (24)

将式(12)代入到式(24)中,可得:
R = c o s θ ∗ I + ( 1 − c o s θ ) ∗ n ⋅ n T + s i n θ ∗ n ∧ R=cos\theta*I+(1-cos\theta)*n\cdot n^T+ sin\theta * n^{\wedge} R=cosθI+(1cosθ)nnT+sinθn (25)

4 拓展

4.1 公式理解和深入

罗德里格斯旋转公式,将3D旋转表示成绕空间中某一旋转轴 n n n旋转角度 θ \theta θ的形式,一般记作:

q = θ ∗ n = [ q x q y q z ] q=\theta*n=\begin{bmatrix} q_{x} \\ q_{y} \\ q_{z} \\ \end{bmatrix} q=θn= qxqyqz (26)

式中, θ = ∣ q ∣ = q x 2 + q y 2 + q z 2 \theta=|q|=\sqrt{q^2_x+q^2_y+q^2_z} θ=q=qx2+qy2+qz2
这种表示形式非常简洁,但存在奇异问题,主要在于:

  1. 旋转角度为 θ \theta θ θ + 2 k π \theta+2k\pi θ+2的结果是一样的;
  2. ( n , θ ) (n,\theta) (n,θ) ( − n , − θ ) (-n,-\theta) (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,\theta)=I+(1-cos\theta)*N^2+ sin\theta * N \\ &\approx I+sin\theta * N\\ &\approx I+\theta * N\\ &=I+q^{\wedge}\\ &=\begin{bmatrix} 1 &-q_{z} &q_{y} \\ q_{z} &1 &-q_{x}\\ -q_{y} &q_{x} &1\\ \end{bmatrix} \end{aligned} R(q)=R(n,θ)=I+(1cosθ)N2+sinθNI+sinθNI+θN=I+q= 1qzqyqz1qxqyqx1 (27)

4.2 极限方法推导罗德里格斯旋转公式

一次性绕旋转轴 n n n旋转角度 θ \theta θ,等价于绕旋转轴 n n n旋转 k k k次,每次旋转角度为 θ / k \theta/k θ/k
因此:

R ( n , θ ) = lim ⁡ k → ∞ ( I + 1 k ∗ θ ∗ N ) k = e θ ∗ N R(n,\theta)=\lim_{k\rightarrow \infty} (I+\frac{1}{k}*\theta*N)^k=e^{\theta*N} R(n,θ)=limk(I+k1θN)k=eθN (28)

而:
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!}+\cdots eθN=I+(θN)+2!(θN)2+3!(θN)3+ (29)

由于:
N k + 2 = − N k , k > 0 N^{k+2}=-N^k,k>0 Nk+2=Nk,k>0 (30)

最终式(29)可简化成如下:
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^{\theta*N}=I+ (\theta-\frac{\theta^3}{3!}+\cdots)*N+ (\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\cdots)*N^2\\ &=I+sin\theta*N+(1-cos\theta)*N^2 \end{aligned} eθN=I+(θ3!θ3+)N+(2!θ24!θ4+)N2=I+sinθN+(1cosθ)N2 (31)

你可能感兴趣的:(数学基础,几何学,线性代数)