世界杯logo

罗德里格斯(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+(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=⎣

⎡​vx​vy​vz​​⎦

⎤​,

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=⎣

⎡​nx​ny​nz​​⎦

⎤​,

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​​−nz​0nx​​ny​−nx​0​⎦

⎤​ (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=⎣

⎡​qx​qy​qz​​⎦

⎤​ (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=⎣

⎡​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​⎦

⎤​ (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=⎣

⎡​nx​ny​nz​​⎦

⎤​,

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}

⎡​ax​ay​az​​⎦

⎤​×⎣

⎡​bx​by​bz​​⎦

⎤​=⎣

⎡​0az​−ay​​−az​0ax​​ay​−ax​0​⎦

⎤​⋅⎣

⎡​bx​by​bz​​⎦

⎤​ (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=⎣

⎡​ax​ay​az​​⎦

⎤​⋅[ax​​ay​​az​​]=⎣

⎡​ax2​ax​ay​ax​az​​ax​ay​ay2​ay​az​​ax​az​ay​az​az2​​⎦

⎤​ (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​​−az​0ax​​ay​−ax​0​⎦

⎤​⋅⎣

⎡​0az​−ay​​−az​0ax​​ay​−ax​0​⎦

⎤​=⎣

⎡​−ay2​−az2​ax​ay​ax​az​​ax​ay​−ax2​−az2​ay​az​​ax​az​ay​az​−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=⎣

⎡​qx​qy​qz​​⎦

⎤​ (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​​−qz​1qx​​qy​−qx​1​⎦

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