三维空间刚体运动2：旋转向量与罗德里格斯公式
三维空间刚体运动2：旋转向量与罗德里格斯公式
- 1.定义
- 2.罗德里格斯公式
-
- 2.1 定义
- 2.2 推导
-
- 2.2.1 推导一
- 2.2.2 推导二
- 2.2.3 推导向量 a a a和 b b b
- 3.旋转矩阵到向量
本篇继续参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动。博文将原第三讲分为四部分来讲解：1、旋转矩阵和变换矩阵；2、旋转向量与罗德里格斯公式；3、欧拉角表示旋转；4、四元数表示变换。本文相对于原文会适当精简，同时为便于理解，会加入一些注解和补充知识点，本篇为第二部分：旋转向量表示旋转，另外三部分请参照博主的其他博文。
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∧。
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.3) v_{\perp } = v - v_{||}\tag{2.3} v⊥=v−v∣∣(2.3)对 v r o t ∣ ∣ v_{rot||} vrot∣∣进行向量分解 v r o t = v r o t ∣ ∣ + v r o t ⊥ (2.4) v_{rot}=v_{rot||}+v_{rot\perp } \tag{2.4} vrot=vrot∣∣+vrot⊥(2.4)下边分别推导 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.5) \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.5} v∣∣=proju(v)=u⋅uu⋅vu=∣∣u∣∣2u⋅vu=(u⋅v)u(2.5)其中 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.6) w=u\times v_{⊥}\tag{2.6} w=u×v⊥(2.6)注意叉乘的顺序，因为我们使用的是右手坐标系统，按照右手定则你可以发现这 个新的向量 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.7) \begin{aligned} ||w|| &= ||u\times v_{⊥}|| \\ &= ||u||\cdot ||v_{⊥}||\cdot \sin(\frac{\pi}{2})\\ &=||v_{⊥}|| \end{aligned}\tag{2.7} ∣∣w∣∣=∣∣u×v⊥∣∣=∣∣u∣∣⋅∣∣v⊥∣∣⋅sin(2π)=∣∣v⊥∣∣(2.7)也就是说， 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.8) \begin{aligned} v_{rot⊥} &= a + b \\ &= \sin \theta w + \cos \theta v_{⊥} \\ &= \sin \theta (u\times v_{⊥}) + \cos \theta v_{⊥} \end{aligned}\tag{2.8} vrot⊥=a+b=sinθw+cosθv⊥=sinθ(u×v⊥)+cosθv⊥(2.8)因为叉乘遵守分配律，且 u u u平行于 v ∣ ∣ v_{||} v∣∣，故： u × v ⊥ = u × ( v − v ∣ ∣ ) = u × v − u × v ∣ ∣ = u × v (2.9) \begin{aligned} u\times v_{⊥} &=u\times (v-v_{||})\\ &=u\times v - u \times v_{||}\\ &=u \times v\end{aligned}\tag{2.9} u×v⊥=u×(v−v∣∣)=u×v−u×v∣∣=u×v(2.9)另外，向量 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.10) \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.10} 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.10) - 显然：到此步，我们还无法将其用矩阵来表示，所以需要对 ( 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.11) (u\cdot v)u=u\cdot(u\cdot v)=u\cdot (u^{T} v)=u\cdot u^{T} v\tag{2.11} (u⋅v)u=u⋅(u⋅v)=u⋅(uTv)=u⋅uTv(2.11)其中的向量都是列向量，点积展开规则为： 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.12) 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.12} u×v=⎣⎡(u×v)x(u×v)y(u×v)z⎦⎤=⎣⎡uyvz−uzvyuzvx−uxvzuxvy−uyvx⎦⎤=⎣⎡0uzuy−uz0uxuy−ux0⎦⎤⎣⎡vxvyvz⎦⎤=Uv(2.12)其中 U = [ 0 − u z u y u z 0 − u x u y u x 0 ] (2.13) U= \begin{bmatrix} 0 & -u_{z} & u_{y}\\ u_{z} & 0 & -u_{x}\\ u_{y} & u_{x} & 0 \end{bmatrix}\tag{2.13} U=⎣⎡0uzuy−uz0uxuy−ux0⎦⎤(2.13) - 将 ( 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.14) \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.14} 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.14)故旋转矩阵 R = cos θ I + ( 1 − cos θ ) u u T + sin θ U . (2.15) R= \cos \theta I +(1-\cos \theta )uu^{T} + \sin \theta U. \tag{2.15} R=cosθI+(1−cosθ)uuT+sinθU.(2.15)其中 I I I为单位矩阵。
2.2.2 推导二
与推导一相比，推导二的不同主要在于用叉乘去表示一些数据。用叉乘来表示 v ⊥ v_{\perp } v⊥： v ⊥ = − u × ( u × v ) (2.16) v_{\perp } =-u\times (u\times v)\tag{2.16} v⊥=−u×(u×v)(2.16)联立推导一中各式得：
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.17) \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.17} 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.17)
从而得出第二种表达式 R = I + ( 1 − cos θ ) U 2 + sin θ U . (2.18) R= I +(1-\cos \theta )U^{2} + \sin \theta U. \tag{2.18} R=I+(1−cosθ)U2+sinθU.(2.18)显然，第二种表达式更为简便，在计算的过程中涉及的参数更少，所以这也是在进行旋转操作时常用的公式。
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.19) \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.19} ∣a∣=sinθ1∣vrot⊥∣=sinθ1∣v⊥∣=sin(π−θ)∣v⊥∣=sinθ∣v⊥∣=sinθsinθ2∣v∣=sinθsinθ2∣v∣∣u∣(2.19)由三角公式 ∣ 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.20) |a|=\sin \theta |u \times v|\tag{2.20} ∣a∣=sinθ∣u×v∣(2.20)
a a a方向
由叉乘方向可得 a a a的单位方向向量为： u × v / ∣ u × v ∣ (2.21) u \times v /|u \times v|\tag{2.21} u×v/∣u×v∣(2.21)
综上可得：
a = ( u × v / ∣ u × v ∣ ) ∣ a ∣ = ( u × v / ∣ u × v ∣ ) sin θ ∣ u × v ∣ = sin θ u × v (2.22) \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.22} a=(u×v/∣u×v∣)∣a∣=(u×v/∣u×v∣)sinθ∣u×v∣=sinθu×v(2.22)
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.23) \begin{aligned} |b| &=\cos \theta_{1}|v_{rot\perp }| \\ &=\cos (\pi -\theta)|v_{\perp }|\\ &= -\cos \theta|v_{\perp }| \end{aligned}\tag{2.23} ∣b∣=cosθ1∣vrot⊥∣=cos(π−θ)∣v⊥∣=−cosθ∣v⊥∣(2.23)
b b b方向
由于 b b b的方向与 v ⊥ v_{\perp } v⊥方向相反，可得 b b b的单位方向向量为： − v ⊥ / ∣ v ⊥ ∣ (2.24) -v_{\perp }/|v_{\perp }|\tag{2.24} −v⊥/∣v⊥∣(2.24)
综上可得：
b = ( − v ⊥ / ∣ v ⊥ ∣ ) ∣ b ∣ = cos θ v ⊥ (2.25) \begin{aligned} b &=(-v_{\perp }/|v_{\perp }|)|b|\\ &=\cos \theta v_{\perp } \end{aligned}\tag{2.25} b=(−v⊥/∣v⊥∣)∣b∣=cosθv⊥(2.25)
至此，罗德里格斯公式的证明全部结束。此外，如果读者希望获得关于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。
至此，推导结束。实践部分代码放到第三部分一起演示。
参考：
1.《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社
2. 罗德里格斯公式推导
3. 四元数与三维旋转