三维空间刚体运动2：旋转向量与罗德里格斯公式（最详推导）
序：本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动，为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解，其中四元数部分较多较复杂，又分为四部分。如果读者急于实践，可直接阅读第五部分的机器人运动轨迹，此部分详细讲解了安装准备工作。此系列总体目录如下：
- 旋转矩阵和变换矩阵；
- 旋转向量表示旋转；
- 欧拉角表示旋转；
- 四元数包括以下部分：
4-1. 四元数表示变换；
4-2. 四元数线性插值方法：LinEuler/LinMat/Lerp/Nlerp/Slerp；
4-3. 四元数多点插值方法：Squad；
4-4. 四元数多点连续解析解插值方法：Spicv；
4-5. 四元数多点离散数值解插值方法：Sping。 - 实践：SLAM中显示机器人运动轨迹及相机位姿。
1.旋转向量定义
对于矩阵表示方式至少有以下两个缺点：
1.
S
O
(
3
)
SO(3)
SO(3)的旋转矩阵有9个量，但一次旋转只有3个自由度，因此这种表达方式是冗余的，同理
S
E
(
4
)
SE(4)
SE(4)也是。那么，是否有更紧凑的表示呢？
2.旋转矩阵和变换矩阵自身带有约束：它必须是正交矩阵且行列式为1。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变的更困难。
因此，希望有一种方式能够紧凑的描述旋转和平移。
旋转向量：事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画，于是，我们可以使用一个向量 u u u（为方便表达，上下文统一采用符号 u u u表示单位向量，其他符号 n , k n,k n,k等也是可以的），其方向与旋转轴一致，其长度等于旋转角 θ \theta θ，那么向量 θ u \theta u θu也可以描述这个旋转，这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转。
在三维空间中定义一个方向只需要用到两个量就可以了（与任意两个坐标轴之间的夹角，比如地球的经纬度就可以确定方向），因此第三个量可以用来定义长度，表示旋转角度。同样，对于变换矩阵，使用一个旋转向量和一个平移向量即可表达一次变换，此时变量维数正好是六维。
2.罗德里格斯公式-向量转换为矩阵
2.1 定义
罗德里格斯公式：旋转向量和旋转矩阵有什么联系吗？事实上，从旋转向量到旋转矩阵的转换过程由罗德里格斯公式（Rodrigues’s Formula）表示。因为任意旋转都可以由一个旋转轴 u u u和一个旋转角 θ \theta θ刻画，故罗德里格斯公式具体形式如下： R = cos θ I + ( 1 − cos θ ) u u T + sin θ u ∧ (2.1) R= \cos \theta I+(1-\cos \theta )uu^{T}+\sin \theta u^{\wedge }\tag{2.1} R=cosθI+(1−cosθ)uuT+sinθu∧(2.1)符号 ∧ ^{\wedge } ∧是向量到反对称矩阵的转换符，见上一篇博客《三维空间刚体运动1：旋转矩阵与变换矩阵》的公式（1.4）。公式还可以写为： R = I + ( 1 − cos θ ) U 2 + sin θ U . (2.2) R= I +(1-\cos \theta )U^{2} + \sin \theta U. \tag{2.2} R=I+(1−cosθ)U2+sinθU.(2.2)其中U代表向量 u u u转换的反对称矩阵 u ∧ u^{\wedge } u∧。假设单位旋转向量 u u u的坐标为 u = [ u x u y u z ] u=\begin{bmatrix} u_{x}\\ u_{y}\\ u_{z} \end{bmatrix} u=⎣⎡uxuyuz⎦⎤，旋转角为 θ \theta θ，则R的展开式为： [ cos θ + u x 2 ( 1 − cos θ ) u x u y ( 1 − cos θ ) − u z sin θ u y sin θ + u x u z ( 1 − cos θ ) u z sin θ + u x u y ( 1 − cos θ ) cos θ + u y 2 ( 1 − cos θ ) − u x sin θ + u y u z ( 1 − cos θ ) − u y sin θ + u x u z ( 1 − cos θ ) u x sin θ + u y u z ( 1 − cos θ ) cos θ + u z 2 ( 1 − cos θ ) ] (2.3) \begin{bmatrix} \cos\theta+u_{x}^{2}(1-\cos\theta) & u_{x}u_{y}(1-\cos\theta)-u_{z}\sin\theta & u_{y}\sin\theta+u_{x}u_{z}(1-\cos\theta)\\ u_{z}\sin\theta+u_{x}u_{y}(1-\cos\theta) & \cos\theta+u_{y}^{2}(1-\cos\theta) & -u_{x}\sin\theta+u_{y}u_{z}(1-\cos\theta)\\ -u_{y}\sin\theta + u_{x}u_{z}(1-\cos\theta) & u_{x}\sin\theta+u_{y}u_{z}(1-\cos\theta) & \cos\theta+u_{z}^{2}(1-\cos\theta) \end{bmatrix}\tag{2.3} ⎣⎡cosθ+ux2(1−cosθ)uzsinθ+uxuy(1−cosθ)−uysinθ+uxuz(1−cosθ)uxuy(1−cosθ)−uzsinθcosθ+uy2(1−cosθ)uxsinθ+uyuz(1−cosθ)uysinθ+uxuz(1−cosθ)−uxsinθ+uyuz(1−cosθ)cosθ+uz2(1−cosθ)⎦⎤(2.3)
2.2 推导
首先，理解下图。定义
u
u
u是旋转轴的单位向量，
v
v
v为旋转向量，
w
w
w为
u
×
v
u\times v
u×v方向上的单位向量。图中
v
v
v绕
u
u
u旋转角度
θ
\theta
θ得到
v
r
o
t
v_{rot}
vrot。将
v
v
v分解为平行于旋转轴
u
u
u以及正交于
u
u
u的两个分量：
v
∥
v_{∥}
v∥和
v
⊥
v_{⊥}
v⊥。将
v
r
o
t
v_{rot}
vrot分解为平行于旋转轴
u
u
u以及正交于
u
u
u的两个分量：
v
r
o
t
∣
∣
v_{rot||}
vrot∣∣和
v
r
o
t
⊥
v_{rot\perp}
vrot⊥，其中
v
r
o
t
∣
∣
=
v
∣
∣
v_{rot||}=v_{||}
vrot∣∣=v∣∣。向量
a
a
a和
b
b
b分别是
v
r
o
t
⊥
v_{rot\perp}
vrot⊥在
w
w
w和
v
⊥
v_{\perp }
v⊥方向上的分量。
图2.1：旋转向量3D图（数学绘图软件推荐geogebra）
所谓推导旋转方程，实则求出一个旋转矩阵，使得
v
r
o
t
=
R
v
v_{rot}=Rv
vrot=Rv，所以我们要做的就是找出
v
r
o
t
v_{rot}
vrot与
v
v
v，并用矩阵来表示。
此公式有2种形式，故而也有2种推导方法，两者推导方法的不同主要在
v
⊥
v_{\perp }
v⊥的表示上。具体推导过程如下。
2.2.1 推导一
推导一推导过程如下：
- v v v：对 v v v进行向量分解： v = v ⊥ + v ∣ ∣ v = v_{\perp } + v_{||} v=v⊥+v∣∣，根据向量减法可得： v ⊥ = v − v ∣ ∣ (2.4) v_{\perp } = v - v_{||}\tag{2.4} v⊥=v−v∣∣(2.4)对 v r o t ∣ ∣ v_{rot||} vrot∣∣进行向量分解 v r o t = v r o t ∣ ∣ + v r o t ⊥ (2.5) v_{rot}=v_{rot||}+v_{rot\perp } \tag{2.5} vrot=vrot∣∣+vrot⊥(2.5)下边分别推导 v r o t ∣ ∣ v_{rot||} vrot∣∣和 v r o t ⊥ v_{rot\perp} vrot⊥。
- v r o t ∣ ∣ v_{rot||} vrot∣∣：由于旋转过程平行向量的不变性可得 v r o t ∣ ∣ = v ∣ ∣ v_{rot||} = v_{||} vrot∣∣=v∣∣， v ∣ ∣ v_{||} v∣∣其实就是 v v v在 u u u上的正交投影（Orthogonal Projection），根据正交投影公式： v ∣ ∣ = p r o j u ( v ) = u ⋅ v u ⋅ u u = u ⋅ v ∣ ∣ u ∣ ∣ 2 u = ( u ⋅ v ) u (2.6) \begin{aligned} v_{||} &= proj_{u}(v) &= \frac{u\cdot v}{u\cdot u}u &= \frac{u\cdot v}{||u||^{2}}u &= (u\cdot v)u \end{aligned}\tag{2.6} v∣∣=proju(v)=u⋅uu⋅vu=∣∣u∣∣2u⋅vu=(u⋅v)u(2.6)其中 u ⋅ u = ∣ ∣ u ∣ ∣ 2 u\cdot u=||u||^{2} u⋅u=∣∣u∣∣2， ∣ ∣ u ∣ ∣ = 1 ||u||=1 ∣∣u∣∣=1，点积 ( u ⋅ v ) (u\cdot v) (u⋅v)为标量，所以再乘向量 u u u得到一个矢量。
-
v
r
o
t
⊥
v_{rot\perp}
vrot⊥：下面画出
v
r
o
t
⊥
v_{rot\perp}
vrot⊥的俯视图：
图2.2：旋转向量俯视图
我们需要处理正交于 u u u的 v ⊥ v_{⊥} v⊥，因为这两个向量是正交的，这个旋转可以看做是平面内的一个旋转，因为旋转不改变 v ⊥ v_{⊥} v⊥的长度，所以路径是一个圆。现在，3D的旋转被转化为了2D平面上的旋转。由于在这个平面上只有一个向量 v ⊥ v_{⊥} v⊥，用它来表示一个旋转是不够的，我们还需要构造一个同时正交于 u u u 和 v ⊥ v_{⊥} v⊥的向量 w w w，这个可以通过叉乘来获得： w = u × v ⊥ (2.7) w=u\times v_{⊥}\tag{2.7} w=u×v⊥(2.7)注意叉乘的顺序，因为我们使用的是右手坐标系统，按照右手定则你可以发现这 个新的向量 w w w指向 v ⊥ v_{⊥} v⊥逆时针旋转 ( π / 2 ) (π/2) (π/2)后的方向，并且和 v ⊥ v_{⊥} v⊥一样也处于正交于 u u u 的平面内。因为 ∥ u ∥ ∥u∥ ∥u∥ = 1，我们可以发现： ∣ ∣ w ∣ ∣ = ∣ ∣ u × v ⊥ ∣ ∣ = ∣ ∣ u ∣ ∣ ⋅ ∣ ∣ v ⊥ ∣ ∣ ⋅ sin ( π 2 ) = ∣ ∣ v ⊥ ∣ ∣ (2.8) \begin{aligned} ||w|| &= ||u\times v_{⊥}|| \\ &= ||u||\cdot ||v_{⊥}||\cdot \sin(\frac{\pi}{2})\\ &=||v_{⊥}|| \end{aligned}\tag{2.8} ∣∣w∣∣=∣∣u×v⊥∣∣=∣∣u∣∣⋅∣∣v⊥∣∣⋅sin(2π)=∣∣v⊥∣∣(2.8)也就是说， w w w和 v ⊥ v_{⊥} v⊥的模长是相同的，所以， w w w也位于圆上。有了这个新的向量 w w w， 就相当于在平面内有了两个坐标轴。我们现在可以把 v r o t ∣ ∣ v_{rot||} vrot∣∣投影到 w w w和 v ⊥ v_{⊥} v⊥上，将其分解为向量 a a a和 b b b，使用一些三角知识可以得到： v r o t ⊥ = a + b = sin θ w + cos θ v ⊥ = sin θ ( u × v ⊥ ) + cos θ v ⊥ (2.9) \begin{aligned} v_{rot⊥} &= a + b \\ &= \sin \theta w + \cos \theta v_{⊥} \\ &= \sin \theta (u\times v_{⊥}) + \cos \theta v_{⊥} \end{aligned}\tag{2.9} vrot⊥=a+b=sinθw+cosθv⊥=sinθ(u×v⊥)+cosθv⊥(2.9)因为叉乘遵守分配律，且 u u u平行于 v ∣ ∣ v_{||} v∣∣，故： u × v ⊥ = u × ( v − v ∣ ∣ ) = u × v − u × v ∣ ∣ = u × v (2.10) \begin{aligned} u\times v_{⊥} &=u\times (v-v_{||})\\ &=u\times v - u \times v_{||}\\ &=u \times v\end{aligned}\tag{2.10} u×v⊥=u×(v−v∣∣)=u×v−u×v∣∣=u×v(2.10)另外，向量 a a a和 b b b还有另外一种证法，稍显繁琐，有兴趣的同学请参见后两小节。
- 综上可得：
v r o t = v r o t ⊥ + v r o t ∣ ∣ = a + b + v ∣ ∣ = sin θ u × v ⊥ + cos θ v ⊥ + ( u ⋅ v ) u = sin θ u × v + cos θ ( v − v ∣ ∣ ) + ( u ⋅ v ) u = sin θ u × v + cos θ ( v − ( u ⋅ v ) u ) + ( u ⋅ v ) u = cos θ v + ( 1 − cos θ ) ( u ⋅ v ) u + sin θ u × v (2.11) \begin{aligned} v_{rot} &= v_{rot\perp}+v_{rot||} \\ &=a+b+v_{||} \\ &=\sin \theta u\times v_{⊥} +\cos \theta v_{\perp } + (u\cdot v)u \\ &=\sin \theta u\times v +\cos \theta (v - v_{||}) + (u\cdot v)u \\ &=\sin \theta u\times v +\cos \theta (v - (u\cdot v)u ) + (u\cdot v)u \\ &=\cos \theta v+(1-\cos \theta )(u\cdot v)u + \sin \theta u\times v \end{aligned}\tag{2.11} vrot=vrot⊥+vrot∣∣=a+b+v∣∣=sinθu×v⊥+cosθv⊥+(u⋅v)u=sinθu×v+cosθ(v−v∣∣)+(u⋅v)u=sinθu×v+cosθ(v−(u⋅v)u)+(u⋅v)u=cosθv+(1−cosθ)(u⋅v)u+sinθu×v(2.11) - 显然：到此步，我们还无法将其用矩阵来表示，所以需要对
(
u
⋅
v
)
u
(u\cdot v)u
(u⋅v)u 和
u
×
v
u\times v
u×v 进行矩阵转换，由点积的交换律和结合律得：
(
u
⋅
v
)
u
=
u
⋅
(
u
⋅
v
)
=
u
⋅
(
u
T
v
)
=
u
⋅
u
T
v
(2.12)
(u\cdot v)u=u\cdot(u\cdot v)=u\cdot (u^{T} v)=u\cdot u^{T} v\tag{2.12}
(u⋅v)u=u⋅(u⋅v)=u⋅(uTv)=u⋅uTv(2.12)其中的向量都是列向量，点积展开规则为：
x
⋅
y
=
[
x
,
y
]
=
x
T
y
x\cdot y = [x, y] =x^{T}y
x⋅y=[x,y]=xTy。
对于 u × v u\times v u×v可用叉乘矩阵来化简为 U v Uv Uv： u × v = [ ( u × v ) x ( u × v ) y ( u × v ) z ] = [ u y v z − u z v y u z v x − u x v z u x v y − u y v x ] = [ 0 − u z u y u z 0 − u x u y u x 0 ] [ v x v y v z ] = U v (2.13) u\times v =\begin{bmatrix} (u\times v)_{x}\\ (u\times v)_{y}\\ (u\times v)_{z} \end{bmatrix}= \begin{bmatrix} u_{y}v_{z}- u_{z}v_{y}\\ u_{z}v_{x}- u_{x}v_{z}\\ u_{x}v_{y}- u_{y}v_{x} \end{bmatrix}= \begin{bmatrix} 0 & -u_{z} & u_{y}\\ u_{z} & 0 & -u_{x}\\ u_{y} & u_{x} & 0 \end{bmatrix}\begin{bmatrix} v_{x}\\ v_{y}\\ v_{z} \end{bmatrix}= Uv\tag{2.13} u×v=⎣⎡(u×v)x(u×v)y(u×v)z⎦⎤=⎣⎡uyvz−uzvyuzvx−uxvzuxvy−uyvx⎦⎤=⎣⎡0uzuy−uz0uxuy−ux0⎦⎤⎣⎡vxvyvz⎦⎤=Uv(2.13)其中 U = [ 0 − u z u y u z 0 − u x u y u x 0 ] (2.14) U= \begin{bmatrix} 0 & -u_{z} & u_{y}\\ u_{z} & 0 & -u_{x}\\ u_{y} & u_{x} & 0 \end{bmatrix}\tag{2.14} U=⎣⎡0uzuy−uz0uxuy−ux0⎦⎤(2.14) - 将 ( u ⋅ v ) u (u\cdot v)u (u⋅v)u 和 u × v u\times v u×v 转换的矩阵代入式（2.10）得： v r o t = cos θ v + ( 1 − cos θ ) ( v ⋅ u ) u + sin θ u × v = cos θ v + ( 1 − cos θ ) u u T v + sin θ U v = ( cos θ I + ( 1 − cos θ ) u u T + sin θ U ) v (2.15) \begin{aligned} v_{rot}&=\cos \theta v+(1-\cos \theta )(v\cdot u)u+ \sin \theta u\times v\\ &=\cos \theta v+(1-\cos \theta )uu^{T}v + \sin \theta U v\\ &=(\cos \theta I +(1-\cos \theta )uu^{T} + \sin \theta U)v \end{aligned}\tag{2.15} vrot=cosθv+(1−cosθ)(v⋅u)u+sinθu×v=cosθv+(1−cosθ)uuTv+sinθUv=(cosθI+(1−cosθ)uuT+sinθU)v(2.15)故旋转矩阵 R = cos θ I + ( 1 − cos θ ) u u T + sin θ U . (2.16) R= \cos \theta I +(1-\cos \theta )uu^{T} + \sin \theta U. \tag{2.16} R=cosθI+(1−cosθ)uuT+sinθU.(2.16)其中 I I I为单位矩阵。
2.2.2 推导二
与推导一相比，推导二的不同主要在于用叉乘去表示一些数据。用叉乘来表示
v
⊥
v_{\perp }
v⊥：
v
⊥
=
−
u
×
(
u
×
v
)
(2.17)
v_{\perp } =-u\times (u\times v)\tag{2.17}
v⊥=−u×(u×v)(2.17)联立推导一中各式得：
v
r
o
t
=
v
r
o
t
⊥
+
v
r
o
t
∣
∣
=
a
+
b
+
v
∣
∣
=
sin
θ
u
×
v
+
cos
θ
v
⊥
+
v
−
v
⊥
=
sin
θ
u
×
v
−
cos
θ
u
×
(
u
×
v
)
+
v
+
u
×
(
u
×
v
)
=
v
+
(
1
−
cos
θ
)
u
×
(
u
×
v
)
+
sin
θ
u
×
v
=
v
+
(
1
−
cos
θ
)
U
2
v
+
sin
θ
U
v
(
叉
乘
矩
阵
表
示
)
=
(
I
+
(
1
−
cos
θ
)
U
2
+
sin
θ
U
)
v
=
R
v
(2.18)
\begin{aligned} v_{rot} &=v_{rot\perp}+v_{rot||}\\ &=a+b+v_{||}\\ &=\sin \theta u\times v +\cos \theta v_{\perp } + v-v_{\perp }\\ &=\sin \theta u\times v -\cos \theta u\times (u\times v) + v + u\times (u\times v)\\ &=v + (1 - \cos \theta )u\times (u\times v) + \sin \theta u\times v \\ &=v + (1 - \cos \theta )U^{2}v+ \sin \theta Uv(叉乘矩阵表示) \\ &=(I + (1 - \cos \theta )U^{2}+ \sin \theta U)v \\ &=Rv \end{aligned}\tag{2.18}
vrot=vrot⊥+vrot∣∣=a+b+v∣∣=sinθu×v+cosθv⊥+v−v⊥=sinθu×v−cosθu×(u×v)+v+u×(u×v)=v+(1−cosθ)u×(u×v)+sinθu×v=v+(1−cosθ)U2v+sinθUv(叉乘矩阵表示)=(I+(1−cosθ)U2+sinθU)v=Rv(2.18)
从而得出第二种表达式
R
=
I
+
(
1
−
cos
θ
)
U
2
+
sin
θ
U
.
(2.19)
R= I +(1-\cos \theta )U^{2} + \sin \theta U. \tag{2.19}
R=I+(1−cosθ)U2+sinθU.(2.19)显然，第二种表达式更为简便，在计算的过程中涉及的参数更少，所以这也是在进行旋转操作时常用的公式。
2.2.3 推导向量 a a a和 b b b
此处单独推导罗德里格斯公式的向量
a
a
a和
b
b
b，仅做参考，也可以忽略不看。
a
a
a和
b
b
b是由
v
r
o
t
⊥
v_{rot\perp }
vrot⊥正交分解得到的矢量，既有大小又有方向，所以在求解时，我们要对其大小和方向分别求解。
a
a
a大小
设
θ
1
=
π
−
θ
\theta_{1}= \pi -\theta
θ1=π−θ，
θ
2
\theta_{2}
θ2是向量
v
v
v和
u
u
u的夹角，
u
u
u为单位向量，则对于
a
a
a的大小
∣
a
∣
|a|
∣a∣有：
∣
a
∣
=
sin
θ
1
∣
v
r
o
t
⊥
∣
=
sin
θ
1
∣
v
⊥
∣
=
sin
(
π
−
θ
)
∣
v
⊥
∣
=
sin
θ
∣
v
⊥
∣
=
sin
θ
sin
θ
2
∣
v
∣
=
sin
θ
sin
θ
2
∣
v
∣
∣
u
∣
(2.20)
\begin{aligned}|a| &=\sin \theta_{1}|v_{rot\perp }|\\ &=\sin \theta_{1}|v_{\perp }| \\ &=\sin (\pi -\theta)|v_{\perp }|\\ &=\sin \theta|v_{\perp }|\\ &=\sin \theta \sin \theta_{2}|v|\\ &=\sin \theta \sin \theta_{2}|v||u| \end{aligned}\tag{2.20}
∣a∣=sinθ1∣vrot⊥∣=sinθ1∣v⊥∣=sin(π−θ)∣v⊥∣=sinθ∣v⊥∣=sinθsinθ2∣v∣=sinθsinθ2∣v∣∣u∣(2.20)由三角公式
∣
a
×
b
∣
=
sin
θ
∣
a
∣
∣
b
∣
|a\times b| = \sin \theta |a||b|
∣a×b∣=sinθ∣a∣∣b∣知：
sin
θ
2
∣
v
∣
∣
u
∣
=
∣
u
×
v
∣
\sin \theta_{2}|v||u|=|u \times v|
sinθ2∣v∣∣u∣=∣u×v∣，所以：
∣
a
∣
=
sin
θ
∣
u
×
v
∣
(2.21)
|a|=\sin \theta |u \times v|\tag{2.21}
∣a∣=sinθ∣u×v∣(2.21)
a
a
a方向
由叉乘方向可得
a
a
a的单位方向向量为：
u
×
v
/
∣
u
×
v
∣
(2.22)
u \times v /|u \times v|\tag{2.22}
u×v/∣u×v∣(2.22)
综上可得：
a
=
(
u
×
v
/
∣
u
×
v
∣
)
∣
a
∣
=
(
u
×
v
/
∣
u
×
v
∣
)
sin
θ
∣
u
×
v
∣
=
sin
θ
u
×
v
(2.23)
\begin{aligned} a&=(u \times v /|u \times v|)|a|\\ &=(u \times v /|u \times v|)\sin \theta |u \times v|\\ &=\sin \theta u\times v \end{aligned}\tag{2.23}
a=(u×v/∣u×v∣)∣a∣=(u×v/∣u×v∣)sinθ∣u×v∣=sinθu×v(2.23)
b
b
b大小
由图得，
θ
1
\theta_{1}
θ1为
b
b
b和
v
r
o
t
⊥
v_{rot\perp }
vrot⊥的夹角，则：
∣
b
∣
=
cos
θ
1
∣
v
r
o
t
⊥
∣
=
cos
(
π
−
θ
)
∣
v
⊥
∣
=
−
cos
θ
∣
v
⊥
∣
(2.24)
\begin{aligned} |b| &=\cos \theta_{1}|v_{rot\perp }| \\ &=\cos (\pi -\theta)|v_{\perp }|\\ &= -\cos \theta|v_{\perp }| \end{aligned}\tag{2.24}
∣b∣=cosθ1∣vrot⊥∣=cos(π−θ)∣v⊥∣=−cosθ∣v⊥∣(2.24)
b
b
b方向
由于
b
b
b的方向与
v
⊥
v_{\perp }
v⊥方向相反，可得
b
b
b的单位方向向量为：
−
v
⊥
/
∣
v
⊥
∣
(2.25)
-v_{\perp }/|v_{\perp }|\tag{2.25}
−v⊥/∣v⊥∣(2.25)
综上可得：
b
=
(
−
v
⊥
/
∣
v
⊥
∣
)
∣
b
∣
=
cos
θ
v
⊥
(2.26)
\begin{aligned} b &=(-v_{\perp }/|v_{\perp }|)|b|\\ &=\cos \theta v_{\perp } \end{aligned}\tag{2.26}
b=(−v⊥/∣v⊥∣)∣b∣=cosθv⊥(2.26)
至此，罗德里格斯公式的证明全部结束。此外，如果读者希望获得关于Oxyz坐标系的旋转变换关系，可以参考这篇博客：图形变换之旋转变换公式推导。
罗德里格斯公式反应的是旋转向量到旋转矩阵的转换关系，如果已知旋转矩阵
R
R
R，如何推导旋转向量
v
v
v呢？下边给出旋转矩阵到旋转向量的反向转换关系。
3.旋转矩阵到旋转向量
这里计算从一个旋转矩阵到旋转向量的转换。对于旋转角 θ \theta θ，取旋转矩阵 R R R两边的迹，有： t r ( R ) = cos θ t r ( I ) + ( 1 − cos θ ) t r ( u u T ) + sin θ t r ( u ∧ ) = 3 cos θ + ( 1 − cos θ ) = 1 + 2 cos θ . (3.1) \begin{aligned} tr(R) &= \cos \theta tr(I)+(1-\cos \theta )tr(uu^{T})+\sin \theta tr(u^{\wedge })\\ &=3\cos \theta + (1- \cos \theta) \\ &=1+2\cos \theta. \tag{3.1} \end{aligned} tr(R)=cosθtr(I)+(1−cosθ)tr(uuT)+sinθtr(u∧)=3cosθ+(1−cosθ)=1+2cosθ.(3.1)因此： θ = arccos t r ( R ) − 1 2 . (3.2) \theta = \arccos \frac{tr(R)-1}{2} .\tag{3.2} θ=arccos2tr(R)−1.(3.2)关于转轴 u u u，旋转轴上的向量在旋转后不发生改变，说明： R u = u . (3.3) Ru=u.\tag{3.3} Ru=u.(3.3)因此，转轴 u u u是矩阵 R R R特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴 u u u。由 v = θ u v=\theta u v=θu得到旋转向量 v v v。
至此，推导结束。实践部分代码放到第三部分一起演示。
本文基于《视觉SLAM十四讲：从理论到实践》和《Quaternions, Interpolation and Animation》编写，但相对于原文会适当精简，同时为便于全面理解，会收集其他网络好文，根据作者理解，加入一些注解和扩展知识点，如果您觉得还不错，请一键四连（点赞关注收藏评论），让更多的人看到。
参考文献：
1.《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社
2. 罗德里格斯公式推导
3. 四元数与三维旋转
5万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
