转载:原文链接 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+(1−cosθ)∗N2⋅v+sinθ∗N⋅v (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+(1−cosθ)∗n⋅nT⋅v+sinθ∗n∧⋅v (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′=⎣ ⎡vx′vy′vz′⎦ ⎤, 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=⎣ ⎡0nz−ny−nz0nxny−nx0⎦ ⎤ (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+(1−cosθ)∗N2+sinθ∗N]⋅v=R⋅v (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+(1−cosθ)∗n⋅nT+sinθ∗n∧]⋅v=R⋅v (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) 旋转矩阵是单位正交矩阵,用于基向量之间的刚性变换(旋转部分),因此满足如下特性: R ⋅ R T = R ⋅ R − 1 = I R\cdot R^{T}=R\cdot R^{-1}=I R⋅RT=R⋅R−1=I; 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) ri⋅rj=0(i,j=1,2,3,i=j); r i ⋅ r i = ∣ r i ∣ = 1 ( i = 1 , 2 , 3 ) r_i\cdot r_i=|r_i|=1\;(i=1,2,3) ri⋅ri=∣ri∣=1(i=1,2,3); ∣ 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) 图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⎦ ⎤=⎣ ⎡0az−ay−az0axay−ax0⎦ ⎤⋅⎣ ⎡bxbybz⎦ ⎤ (8) 上式可被简写成: a × b = a ∧ ⋅ b = A ⋅ b a\times b=a^{\wedge}\cdot b=A\cdot b a×b=a∧⋅b=A⋅b (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} a⋅aT=⎣ ⎡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} A⋅A=⎣ ⎡0az−ay−az0axay−ax0⎦ ⎤⋅⎣ ⎡0az−ay−az0axay−ax0⎦ ⎤=⎣ ⎡−ay2−az2axayaxazaxay−ax2−az2ayazaxazayaz−ax2−ay2⎦ ⎤ (11) 因此,对于单位向量,满足: a ⋅ a T = I + A ⋅ A a\cdot a^{T}=I+A\cdot A a⋅aT=I+A⋅A (12) 3 罗德里格斯旋转公式推导 3.1 符号说明 图2 向量旋转示意图 对图2中的相关符号作如下说明: 注:旋转平面是与旋转轴相垂直的平面。 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∥′=v−v⊥=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⊥′=∣v⊥′∣∗cosθ∗∣v⊥∣v⊥+∣v⊥′∣∗sinθ∗∣w∣w=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+v−v⊥=(1−cosθ)∗n×(n×v)+sinθ∗n×v+v=(1−cosθ)∗N2⋅v+sinθ∗N⋅v+v=[(1−cosθ)∗N2+sinθ∗N+I]⋅v=R⋅v (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+(1−cosθ)∗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+(1−cosθ)∗n⋅nT+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 。 这种表示形式非常简洁,但存在奇异问题,主要在于: 旋转角度为 θ \theta θ和 θ + 2 k π \theta+2k\pi θ+2kπ的结果是一样的; ( 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+(1−cosθ)∗N2+sinθ∗N≈I+sinθ∗N≈I+θ∗N=I+q∧=⎣ ⎡1qz−qy−qz1qxqy−qx1⎦ ⎤ (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!θ2−4!θ4+⋯)∗N2=I+sinθ∗N+(1−cosθ)∗N2 (31)