三维空间刚体运动2：旋转向量与罗德里格斯公式（最详推导）

1.旋转向量定义
2.罗德里格斯公式-向量转换为矩阵
- 2.1 定义
- 2.2 推导
- - 2.2.1 推导一
  - 2.2.2 推导二
  - 2.2.3 推导向量 a a a和 b b b
3.旋转矩阵到旋转向量

序：本篇系列文章参照高翔老师《视觉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. 四元数与三维旋转

三维空间刚体运动2：旋转向量与罗德里格斯公式（最详细推导）相关推荐

《视觉SLAM十四讲》-第三章第3节公式推导-旋转向量-欧拉角--罗德里格斯公式详细推导
三维空间刚体运动4-1：四元数表示旋转（各形式相互转换加代码）
三维空间刚体运动4-1:四元数表示变换(各形式相互转换加代码) 1. 四元数的定义 1.1 为什么使用四元数 1.2 复数与四元数 1.3 四元数的形式 2. 四元数的运算 2.1 基础运算 2.2 ...
视觉SLAM十四讲第3讲三维空间刚体运动（相关知识点汇总）
视觉SLAM十四讲第3讲三维空间刚体运动 1. 刚体 2. 欧氏空间(euclidean space) 2.1 欧氏距离: 2.2 欧氏变换: 3. 笛卡尔坐标系 4. 透视空间 5. 齐次坐标系 ...
三维空间刚体运动3：欧拉角表示旋转（全面理解万向锁、RPY角和欧拉角）
三维空间刚体运动3:欧拉角表示旋转(全面理解万向锁.RPY角和欧拉角) 1. 欧拉角 1.1 定义 2.2 RPY角与Z-Y-X欧拉角 2. 欧拉角到旋转矩阵 3. 旋转矩阵到欧拉角 4. 万向锁 4 ...
视觉SLAM十四讲：第3讲三维空间刚体运动
第3讲:三维空间刚体运动三维空间中刚体运动的描述方式:旋转矩阵.变换矩阵.四元数和欧拉角 3.1 旋转矩阵 3.1.1 点和向量,坐标系三维空间中,给定线性空间基(e1,e2,e3)(\mathb ...
高博14讲--第三讲三维空间刚体运动
高博14讲--第三讲三维空间刚体运动旋转矩阵点和向量.坐标系坐标系间的欧式变换变换矩阵与齐次坐标旋转向量和欧拉角旋转向量欧拉角四元数四元数的定义四元数的运算用四元数表示旋转四 ...
三维空间刚体运动4-5：四元数多点离散数值解插值方法：Sping
三维空间刚体运动4-5:四元数多点离散数值解插值方法:Sping 1. 正切曲率κ(γ,t)\kappa(\gamma, t)κ(γ,t)在H1H_{1}H1上的离散数值解--Sping 1.1 离 ...
三维空间刚体运动4-4：四元数多点连续解析解插值方法：Spicv
三维空间刚体运动4-4:四元数多点连续解析解插值方法:Spicv 1. 总述:多点旋转插值的数学方法 2. 插值曲线及其连续性 2.1 插值曲线定义 2.2 插值曲线连续性的讨论 3. 最优插值曲线 ...
三维空间刚体运动4-3：四元数线性插值方法：Squad
三维空间刚体运动4-3:四元数线性插值方法:Squad Squad的引出 B e ˊ z i e r c u r v e B\acute{e}zier \space curveB e ˊ zier c ...

三维空间刚体运动2：旋转向量与罗德里格斯公式（最详细推导）