关于罗德里格斯公式[Rodrigues'sFormula]的详细推导过程
1 旋转向量
实际上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们可以使用一个向量 u u u，其方向与旋转轴一致，其长度等于旋转角 θ \theta θ，那么向量 θ u \theta u θu就可以描述这个旋转，这种向量称为旋转向量(或轴角/角轴，Axis-Angle)，只需一个三维向量即可描述旋转。同样，对于一个变换矩阵，使用一个旋转向量和一个平移向量即可表达一次变换，此时变量维数正好是六维。
2 罗德里格斯公式
2.1 罗德里格斯公式定义
维基百科给出的罗德里格斯旋转公式(Rodrigues’ rotation formula)定义如下：
如果
v
v
v是
R
3
\mathbb{R}^3
R3中的向量，并且
k
k
k是描述旋转轴的单位向量，根据右手法则
v
v
v围绕该轴旋转角度
θ
\theta
θ，则旋转向量
v
r
o
t
v_{rot}
vrot的罗德里格斯公式为
v
r
o
t
=
v
c
o
s
θ
+
(
k
×
v
)
s
i
n
θ
+
k
(
k
⋅
v
)
(
1
−
c
o
s
θ
)
.
v_{rot}=vcos\theta+(k \times v)sin \theta +k(k\cdot v)(1-cos\theta).
vrot=vcosθ+(k×v)sinθ+k(k⋅v)(1−cosθ).视觉SLAM十四讲中给出的用罗德里格斯公式转换的结果为：
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
∧
R=cos\theta I+(1-cos\theta)nn^T+sin\theta n^{\land}
R=cosθI+(1−cosθ)nnT+sinθn∧其中，
R
R
R表示一个旋转，单位长度的向量
n
n
n为一个假设的旋转轴，角度为
θ
\theta
θ，符号
∧
^\land
∧是向量到反对称矩阵的转换符，具体含义见文章：[视觉SLAM十四讲]学习笔记1-刚体运动之旋转矩阵与变换矩阵。
我们可以看出，原始的定义和书中给出的转换结果形似，这说明这两个公式有一定的关系，下一节将通过推导过程来展示这个关系的转换。
2.2 罗德里格斯公式推导
我们可以通过三维空间中的旋转和旋转分解来进行罗德里格斯公式的推导。
首先，我们给出一些定义：令
k
k
k为定义旋转轴的单位向量，令
v
v
v为围绕
k
k
k旋转角度
θ
\theta
θ的任意向量(遵循右手法则，图中为逆时针方向)。
在图中，向量
v
v
v围绕
k
k
k旋转
θ
\theta
θ度得到向量
v
r
o
t
v_{rot}
vrot，将向量
v
v
v分解为平行于旋转轴
k
k
k的向量
v
∣
∣
v_{||}
v∣∣和正交于(垂直)旋转轴
k
k
k的向量
v
⊥
v_{⊥}
v⊥，
v
=
v
∣
∣
+
v
⊥
v=v_{||}+v_{⊥}
v=v∣∣+v⊥。
简单来说，根据2.1中的转换结果和下图可知，
v
r
o
t
=
R
v
v_{rot}=Rv
vrot=Rv，为了求解出转换关系即
R
R
R我们只需要找到
v
r
o
t
v_{rot}
vrot的表达式即可。
根据下图，我们可以看出，向量
v
r
o
t
v_{rot}
vrot同样被分解成了两个向量
v
∣
∣
r
o
t
v_{||rot}
v∣∣rot和
v
⊥
r
o
t
v_{⊥rot}
v⊥rot，但这幅图不便于我们进行公式的推导和观察，我们可以换一种分解方式，类似于第一个图，这时向量
v
∣
∣
v_{||}
v∣∣和向量
v
∣
∣
r
o
t
v_{||rot}
v∣∣rot将平行(重合)于旋转轴
k
k
k。
1.
v
∣
∣
v_{||}
v∣∣的旋转及
v
r
o
t
∣
∣
v_{rot||}
vrot∣∣的表示
首先我们看一下向量
v
v
v的分解示意图(图中
u
u
u为旋转轴)：
通过上图，我们可以看到，
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
v_{||}=proj_{u}(v)=\frac{u\cdot v}{u \cdot u}u=\frac{u\cdot v}{\Vert u \Vert^2}u=(u\cdot v)u
v∣∣=proju(v)=u⋅uu⋅vu=∥u∥2u⋅vu=(u⋅v)u这里，
∥
u
∥
2
=
u
⋅
u
，
∥
u
∥
=
1
\Vert u \Vert^2=u \cdot u，\Vert u \Vert=1
∥u∥2=u⋅u，∥u∥=1，从而由
v
=
v
∣
∣
+
v
⊥
v=v_{||}+v_{⊥}
v=v∣∣+v⊥，我们可以得到
v
⊥
=
v
−
v
∣
∣
=
v
−
(
u
⋅
v
)
u
v_{⊥}=v-v_{||}=v-(u\cdot v)u
v⊥=v−v∣∣=v−(u⋅v)u，，所以
v
⊥
=
v
−
v
∣
∣
=
v
−
(
u
⋅
v
)
u
v_{⊥}=v-v_{||}=v-(u\cdot v)u
v⊥=v−v∣∣=v−(u⋅v)u。
从之前的图中我们可以看到，
v
∣
∣
v_{||}
v∣∣其实没有被旋转，仍然与旋转轴
u
u
u重合，所以：
v
∣
∣
=
v
∣
∣
r
o
t
v_{||}=v_{||rot}
v∣∣=v∣∣rot。
2.
v
⊥
v_{⊥}
v⊥的旋转及
v
⊥
r
o
t
v_{⊥rot}
v⊥rot的表示
通过之前的图，我们可以看出因向量
v
⊥
v_{⊥}
v⊥正交于向量
u
u
u，这个旋转可以看做是平面内的一个旋转。因为旋转不改变
v
⊥
v_{⊥}
v⊥的长度，所以路径是一个圆。通过下图我们可以清晰的看到旋转的情况(第一个图为旋转情况，第二个图为旋转情况的俯视图)：
有了旋转情况的俯视图，我们就可以通过平面上的旋转来分析三维旋转的情况了，为了表示旋转，我们可以构造一个同时正交于
u
u
u和
v
⊥
v_{⊥}
v⊥的向量
w
w
w，通过外积(叉乘)，我们可以表示出来
w
=
u
×
v
⊥
w=u \times v_{⊥}
w=u×v⊥，由于我们一直使用的是右手坐标系统，所以在表示时要注意叉乘顺序。按照右手定则，新定义的向量
w
w
w指向的是
v
⊥
v_{⊥}
v⊥逆时针旋转
π
/
2
\pi /2
π/2后的方向，同时和
v
⊥
v_{⊥}
v⊥一样位于正交于
u
u
u的平面内，由
∥
u
∥
=
1
\Vert u \Vert=1
∥u∥=1，我们可以得到以下关系：
∥
w
∥
=
∥
u
×
v
⊥
∥
=
∥
u
∥
⋅
∥
v
⊥
∥
⋅
s
i
n
(
π
/
2
)
=
∥
v
⊥
∥
.
\Vert w \Vert=\Vert u \times v_{⊥}\Vert=\Vert u\Vert \cdot \Vert v_{⊥}\Vert \cdot sin(\pi/2)=\Vert v_{⊥}\Vert.
∥w∥=∥u×v⊥∥=∥u∥⋅∥v⊥∥⋅sin(π/2)=∥v⊥∥.这里
π
/
2
\pi/2
π/2是
u
u
u与
v
⊥
v_{⊥}
v⊥的夹角，同时我们可以看出，
w
w
w和
v
⊥
v_{⊥}
v⊥具有相同的模长，即
w
w
w也位于这个圆上，
w
w
w和
v
⊥
v_{⊥}
v⊥即成为了一个平面上的两个坐标轴。这样，我们就可以把
v
⊥
r
o
t
v_{⊥rot}
v⊥rot投影到
w
w
w和
v
⊥
v_{⊥}
v⊥上，我们可以将
v
⊥
r
o
t
v_{⊥rot}
v⊥rot分解为
v
r
w
v_{rw}
vrw和
v
r
v
v_{rv}
vrv，通过三角函数的一些知识，我们可以得到以下关系：
v
⊥
r
o
t
=
v
r
v
+
v
r
w
=
c
o
s
(
θ
)
v
⊥
+
s
i
n
(
θ
)
w
=
c
o
s
(
θ
)
v
⊥
+
s
i
n
(
θ
)
(
u
×
v
⊥
)
v_{⊥rot}=v_{rv}+v_{rw}=cos(\theta)v_{⊥}+sin(\theta)w=cos(\theta)v_{⊥}+sin(\theta)(u \times v_{⊥})
v⊥rot=vrv+vrw=cos(θ)v⊥+sin(θ)w=cos(θ)v⊥+sin(θ)(u×v⊥)
3.
v
r
o
t
v_{rot}
vrot的表示
通过1和2得到的结果，我们可以得到
v
r
o
t
v_{rot}
vrot的表达式为：
v
r
o
t
=
v
r
o
t
∣
∣
+
v
r
o
t
⊥
=
v
∣
∣
+
c
o
s
(
θ
)
v
⊥
+
s
i
n
(
θ
)
(
u
×
v
⊥
)
v_{rot}=v_{rot||}+v_{rot⊥}=v_{||}+cos(\theta)v_{⊥}+sin(\theta)(u \times v_{⊥})
vrot=vrot∣∣+vrot⊥=v∣∣+cos(θ)v⊥+sin(θ)(u×v⊥)对于
u
×
v
⊥
u \times v_{⊥}
u×v⊥，我们可以采用分配律进行变形
u
×
v
⊥
=
u
×
(
v
−
v
∣
∣
)
=
u
×
v
−
u
×
v
∣
∣
=
u
×
v
u \times v_{⊥}=u \times (v-v_{||})=u \times v-u \times v_{||}=u \times v
u×v⊥=u×(v−v∣∣)=u×v−u×v∣∣=u×v这里
u
u
u和
v
∣
∣
v_{||}
v∣∣平行，所以
u
×
v
∣
∣
=
0
u \times v_{||}=0
u×v∣∣=0。
最后代入
v
∣
∣
=
(
u
⋅
v
)
u
v_{||}=(u \cdot v)u
v∣∣=(u⋅v)u和
v
⊥
=
v
−
(
u
⋅
v
)
u
v_{⊥}=v-(u \cdot v)u
v⊥=v−(u⋅v)u，得到下面的结果
v
r
o
t
=
(
u
⋅
v
)
u
+
c
o
s
(
θ
)
(
v
−
(
u
⋅
v
)
u
)
+
s
i
n
(
θ
)
(
u
×
v
)
v_{rot}=(u \cdot v)u+cos(\theta)(v-(u \cdot v)u)+sin(\theta)(u \times v)
vrot=(u⋅v)u+cos(θ)(v−(u⋅v)u)+sin(θ)(u×v)
=
v
c
o
s
(
θ
)
+
(
u
×
v
)
s
i
n
(
θ
)
+
(
u
⋅
v
)
u
(
1
−
c
o
s
(
θ
)
)
=vcos(\theta)+(u \times v)sin(\theta)+(u \cdot v)u(1-cos(\theta))
=vcos(θ)+(u×v)sin(θ)+(u⋅v)u(1−cos(θ))这样，我们就得到了3D空间中任意一个向量
v
v
v沿着单位向量(旋转轴)
u
u
u旋转
θ
\theta
θ角度之后的
v
r
o
t
v_{rot}
vrot。
到这里，我们推导出了罗德里格斯公式的基本形式，但显然，书中给出的旋转
R
R
R是矩阵形式，接下来我们要将内积和外积的运算进行变形，用矩阵形式表示。
对于
(
u
⋅
v
)
u
(u \cdot v)u
(u⋅v)u，通过内积的展开规则
x
⋅
y
=
[
x
,
y
]
=
x
T
y
x \cdot y=[x,y]=x^Ty
x⋅y=[x,y]=xTy及交换律可以得到
(
u
⋅
v
)
u
=
(
u
⋅
u
)
v
=
(
u
⋅
u
T
)
v
=
u
u
T
v
(u \cdot v)u=(u \cdot u)v=(u \cdot u^T)v=uu^Tv
(u⋅v)u=(u⋅u)v=(u⋅uT)v=uuTv同时，
u
×
v
u \times v
u×v可以通过之前在：[视觉SLAM十四讲]学习笔记1-刚体运动之旋转矩阵与变换矩阵中介绍的叉乘矩阵的形式，即为
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
−
k
z
k
y
k
z
0
−
k
x
−
k
y
k
x
0
]
[
v
x
v
y
v
z
]
=
U
v
u \times v=\begin{bmatrix}(u \times v)_x \\ (u \times v)_y \\ (u \times v)_z \end{bmatrix}=\begin{bmatrix}u_yv_z-u_zv_y \\u_zv_x-u_xv_z \\u_xv_y-u_yv_x \end{bmatrix}=\begin{bmatrix}0&-k_z&k_y \\ k_z&0&-k_x \\ -k_y&k_x&0\end{bmatrix}\begin{bmatrix}v_x \\ v_y \\ v_z \end{bmatrix}=Uv
u×v=
(u×v)x(u×v)y(u×v)z
=
uyvz−uzvyuzvx−uxvzuxvy−uyvx
=
0kz−ky−kz0kxky−kx0
vxvyvz
=Uv这里
U
=
u
∧
=
[
0
−
k
z
k
y
k
z
0
−
k
x
−
k
y
k
x
0
]
U=u^{\land}=\begin{bmatrix}0&-k_z&k_y \\ k_z&0&-k_x \\ -k_y&k_x&0\end{bmatrix}
U=u∧=
0kz−ky−kz0kxky−kx0
将转换后的
(
u
⋅
v
)
u
(u \cdot v)u
(u⋅v)u和
u
×
v
u \times v
u×v矩阵形式带入推导的结果，变形结果如下：
v
r
o
t
=
v
c
o
s
(
θ
)
+
(
u
×
v
)
s
i
n
(
θ
)
+
(
u
⋅
v
)
u
(
1
−
c
o
s
(
θ
)
)
v_{rot}=vcos(\theta)+(u \times v)sin(\theta)+(u \cdot v)u(1-cos(\theta))
vrot=vcos(θ)+(u×v)sin(θ)+(u⋅v)u(1−cos(θ))
=
v
c
o
s
(
θ
)
+
(
1
−
c
o
s
(
θ
)
)
u
u
T
v
+
s
i
n
(
θ
)
u
∧
v
=vcos(\theta)+(1-cos(\theta))uu^Tv+sin(\theta)u^{\land}v
=vcos(θ)+(1−cos(θ))uuTv+sin(θ)u∧v
=
(
c
o
s
(
θ
)
I
+
(
1
−
c
o
s
(
θ
)
)
u
u
T
+
s
i
n
(
θ
)
u
∧
)
v
=(cos(\theta)I+(1-cos(\theta))uu^T+sin(\theta)u^{\land})v
=(cos(θ)I+(1−cos(θ))uuT+sin(θ)u∧)v则旋转矩阵为
R
=
c
o
s
(
θ
)
I
+
(
1
−
c
o
s
(
θ
)
)
u
u
T
+
s
i
n
(
θ
)
u
∧
R=cos(\theta)I+(1-cos(\theta))uu^T+sin(\theta)u^{\land}
R=cos(θ)I+(1−cos(θ))uuT+sin(θ)u∧
I
I
I为单位矩阵，证毕。
3 旋转矩阵到旋转向量的转换
有了上节的推导和转换结果，我们可以在转换公式的基础上，进行旋转矩阵到旋转向量的转换。
首先对转换公式两边取迹，得
t
r
(
R
)
=
c
o
s
(
θ
)
t
r
(
I
)
+
(
1
−
c
o
s
(
θ
)
)
t
r
(
u
u
T
)
+
s
i
n
(
θ
)
t
r
(
u
∧
)
tr(R)=cos(\theta)tr(I)+(1-cos(\theta))tr(uu^T)+sin(\theta)tr(u^{\land})
tr(R)=cos(θ)tr(I)+(1−cos(θ))tr(uuT)+sin(θ)tr(u∧)
=
3
c
o
s
(
θ
)
+
(
1
−
c
o
s
(
θ
)
=3cos(\theta)+(1-cos(\theta)
=3cos(θ)+(1−cos(θ)
=
1
+
2
c
o
s
(
θ
)
=1+2cos(\theta)
=1+2cos(θ)因此：
θ
=
a
r
c
c
o
s
t
r
(
R
)
−
1
2
\theta =arccos\frac{tr(R)-1}{2}
θ=arccos2tr(R)−1对于旋转轴
u
u
u和之前的图及结果，我们可以知道，旋转轴上的向量在旋转后不会发生改变，说明：
R
u
=
u
Ru=u
Ru=u显然，旋转轴
u
u
u是旋转矩阵
R
R
R特征值
I
I
I对应的特征向量。求解这个方程，进行归一化，就可以得到旋转轴。
参考资料：
视觉SLAM十四讲：从理论到实践(第2版)(ISBN：9787121369421)
维基百科-罗德里格斯的旋转公式
四元数与三维旋转
977
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
