最近在进行项目的时候需要将两个面进行旋转统一到一个面，但matlab中并没有一个合适的库函数去做这件事，于是自己开发，但网上很多对罗德里格公式的推导并不是很清晰，各关系式之间的逻辑关系之间的阐述有断层，也没有配相关的代码解析。
在开始推导之前，可以先带着一个问题，加入你有两个单位向量，如何把他们旋转到重合？
看上图，初始我们有向量V和单位向量K，V围绕单位向量K旋转角度为θ得到的向量为Vrot，我们将向量V和Vrot进行分解，如上图所示（该图出自维基百科，如果看不到可以搜索罗德里格矩阵公式）。
V
=
V
T
+
V
Ⅱ
(
1
)
V
r
o
t
=
V
T
t
o
r
+
V
Ⅱ
r
o
t
(
2
)
V=V_T+V_Ⅱ \quad (1)\\ V_{rot}=V_{Ttor}+V_{Ⅱrot} \quad (2)
V=VT+VⅡ(1)Vrot=VTtor+VⅡrot(2)
其中很明显的一点是VⅡ与VⅡrot是相等的。因为向量V在围绕单位向量K旋转的过程中K方向的分量（范数和方向)不会改变。
由于K是单位向量，所以VⅡ与K以及V之间的关系为
V
Ⅱ
=
(
V
⋅
K
)
K
（
3
)
其中
(
V
⋅
K
)
是
V
在单位向量
K
上的投影
V_{Ⅱ}=(V\cdot K)K \quad（3) \\其中(V\cdot K)是 V在单位向量K上的投影
VⅡ=(V⋅K)K（3)其中(V⋅K)是V在单位向量K上的投影
我们讲公式3与公式1结合即（1）公式两侧同时减去（3）那么得到下面一个公式
V
T
=
V
−
(
V
⋅
K
)
K
将
V
的左边乘以
(
K
⋅
K
)
,
上式就变为
V
T
=
(
K
⋅
K
)
V
−
(
V
⋅
K
)
K
(
4
)
根据叉积公式
A
×
(
B
×
C
)
=
(
A
⋅
C
)
B
−
(
A
⋅
B
)
C
所以（
4
）式就变为
V
T
=
K
×
(
V
×
K
)
(
5
)
V_T=V-(V\cdot K)K \\ 将V的左边乘以(K\cdot K) ,上式就变为\\ V_T=(K\cdot K)V-(V \cdot K) K \quad (4)\\ 根据叉积公式 A \times (B \times C)=(A\cdot C)B-(A\cdot B)C\\ 所以（4）式就变为 \\ V_T=K\times(V\times K) \quad(5)
VT=V−(V⋅K)K将V的左边乘以(K⋅K),上式就变为VT=(K⋅K)V−(V⋅K)K(4)根据叉积公式A×(B×C)=(A⋅C)B−(A⋅B)C所以（4）式就变为VT=K×(V×K)(5)
下面有很重要的一个点式很多说明罗德里格公式里没有说明的：
向量V与单位向量K的叉积得到的新向量与VT成90°，且两者的模相等，这是由于（你可以证明一下，这很简单）
∣
K
×
V
∣
=
∣
K
∣
⋅
∣
V
∣
s
i
n
(
δ
)
δ
为
K
V
之间的夹角
|K\times V|=|K|\cdot|V|sin(\delta) \quad \delta 为K \quad V之间的夹角
∣K×V∣=∣K∣⋅∣V∣sin(δ)δ为KV之间的夹角
现在我们看如何用VT ，K，V来表达VTrot：
由上图可得
V
T
r
o
t
=
V
T
c
o
s
(
θ
)
+
K
×
V
s
i
n
(
θ
)
(
6
)
V_{Trot}=V_Tcos(\theta)+K\times V sin(\theta) \quad (6)
VTrot=VTcos(θ)+K×Vsin(θ)(6)
再讲（6）式与（2）式结合
V
r
o
t
=
V
T
cos
(
θ
)
+
K
×
V
sin
(
θ
)
+
V
Ⅱ
将
V
T
用
(
V
−
V
Ⅱ
)
替换，上式将变为
V
r
o
t
=
(
V
−
V
Ⅱ
）
cos
(
θ
)
+
K
×
V
sin
(
θ
)
+
V
Ⅱ
将括号拆开并重新整合
V
r
o
t
=
(
1
−
cos
(
θ
)
)
V
Ⅱ
+
cos
(
θ
)
V
+
sin
(
θ
)
(
K
×
V
)
(
7
)
V_{rot}=V_{T}\cos(\theta)+K\times V \sin(\theta)+V{Ⅱ}\\将V_T用(V-V_Ⅱ)替换，上式将变为\\ V_{rot}=(V-V_Ⅱ）\cos(\theta)+K\times V \sin(\theta)+V{Ⅱ}\quad 将括号拆开并重新整合\\ V_{rot}=(1-\cos(\theta))V_Ⅱ+\cos(\theta)V+\sin(\theta)(K\times V) \quad (7)
Vrot=VTcos(θ)+K×Vsin(θ)+VⅡ将VT用(V−VⅡ)替换，上式将变为Vrot=(V−VⅡ）cos(θ)+K×Vsin(θ)+VⅡ将括号拆开并重新整合Vrot=(1−cos(θ))VⅡ+cos(θ)V+sin(θ)(K×V)(7)
将7式中的VⅡ用（3）式替换消元，且在等式右侧加减V,则变为下式：
V
r
o
t
=
V
−
V
+
(
1
−
cos
(
θ
)
)
(
V
⋅
K
)
K
+
cos
(
θ
)
V
+
sin
(
θ
)
(
K
×
V
)
=
V
−
(
1
−
cos
(
θ
)
)
V
+
(
1
−
cos
(
θ
)
)
(
V
⋅
K
)
K
+
sin
(
θ
)
(
K
×
V
)
=
V
+
[
(
1
−
cos
(
θ
)
)
(
(
V
⋅
K
)
K
−
(
K
⋅
K
)
V
]
+
sin
(
θ
)
(
K
×
V
)
=
V
+
(
1
+
cos
(
θ
)
)
[
K
×
(
K
×
V
)
]
+
sin
(
θ
)
(
K
×
V
)
(
8
)
V_{rot}=V-V+(1-\cos(\theta))(V \cdot K) K+\cos(\theta)V+\sin(\theta)(K\times V) \\ =V-(1-\cos(\theta))V+(1-\cos(\theta))(V\cdot K)K +\sin(\theta)(K\times V)\\ =V+[(1-\cos(\theta))((V \cdot K)K-(K\cdot K)V]+\sin(\theta)(K\times V)\\ =V+(1+\cos(\theta))[K\times(K\times V)]+\sin(\theta)(K\times V) \quad(8)
Vrot=V−V+(1−cos(θ))(V⋅K)K+cos(θ)V+sin(θ)(K×V)=V−(1−cos(θ))V+(1−cos(θ))(V⋅K)K+sin(θ)(K×V)=V+[(1−cos(θ))((V⋅K)K−(K⋅K)V]+sin(θ)(K×V)=V+(1+cos(θ))[K×(K×V)]+sin(θ)(K×V)(8)
下面介绍叉乘矩阵（这个我不太熟悉，你如果想深入了解，可以看一下别人的博客，很有意思 推荐博客
那么我们使Rk为K x V的叉乘矩阵 则K x V就变为了RkV，而K x(K x V)经过两次变换就成为了Rk2
将8式通过叉乘矩阵整合之后可得
V
r
o
t
=
V
+
(
1
+
cos
(
θ
)
)
R
k
2
V
+
sin
(
θ
)
V
=
[
1
+
(
1
+
cos
(
θ
)
)
R
k
2
+
R
k
]
V
(
9
)
V_{rot}=V+(1+\cos(\theta))R_k^2 V+\sin(\theta)V\\ =[1+(1+\cos(\theta))R_k^2+R_k]V \quad(9)
Vrot=V+(1+cos(θ))Rk2V+sin(θ)V=[1+(1+cos(θ))Rk2+Rk]V(9)
而9式就是我们常见的罗德里格旋转矩阵的公式了。是不是很简单
下面贴出罗德里格旋转矩阵的Matlab实现：
function rat=rotate(b,a) %the rotate matrix for rot*a'=b'
b=b/norm(b);%将传入的向量单位化
a=a/norm(a);
rotationangle=acos(dot(a,b));%求出两向量之间的夹角
rotationaxis=cross(a,b);%求出他们的旋转轴，也是两个向量构成的法向量
rotationaxis=rotationaxis/norm(rotationaxis);%将旋转轴单位化
matrix=zeros(3,3);
matrix(1,2)=-rotationaxis(1,3);%设计叉乘矩阵
matrix(1,3)=rotationaxis(1,2);
matrix(2,1)=rotationaxis(1,3);
matrix(2,3)=-rotationaxis(1,1);
matrix(3,1)=-rotationaxis(1,2);
matrix(3,2)=rotationaxis(1,1);
rat=eye(3)+(1-cos(rotationangle))*matrix*matrix+sin(rotationangle)*matrix;%应用罗德里格公式求出旋转矩阵
end
2831
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
