CH3 三维空间刚体运动 笔记
旋转矩阵
-
向量在指定坐标系下用坐标表示
a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 \boldsymbol{a}=[\boldsymbol{e}_1,\boldsymbol{e}_2,\boldsymbol{e}_3]\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=a_1\boldsymbol{e}_1+a_2\boldsymbol{e}_2+a_3\boldsymbol{e}_3 a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3 -
内积
a ⋅ b = a T b = ∑ i = 1 3 a i b i = ∣ a ∣ ∣ b ∣ c o s ⟨ a , b ⟩ \boldsymbol{a}·\boldsymbol{b}=\boldsymbol{a}^T\boldsymbol{b}=\sum_{i=1}^3a_ib_i=|a||b|cos\langle\boldsymbol{a},\boldsymbol{b}\rangle a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,b⟩ -
外积
两个向量的外积,方向垂直于这两个向量,大小等于两个向量张成的四边形的面积
a × b = [ i j k a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b ≜ a ∧ b \boldsymbol{a}\times\boldsymbol{b}=\begin{bmatrix}i&j&k \\a_1&a_2&a_3\\b_1&b_2&b_3\end{bmatrix}=\begin{bmatrix}a_2b_3-a_3b_2\\a_3b_1-a_1b_3\\a_1b_2-a_2b_1\end{bmatrix}=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}\boldsymbol{b}\triangleq\boldsymbol{a}^{\wedge}\boldsymbol{b} a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b -
反对称矩阵
将外积转化为了矩阵和向量的乘法,变成了线性运算。
a × b = a ∧ b \boldsymbol{a}\times\boldsymbol{b}=\boldsymbol{a}^{\wedge}\boldsymbol{b} a×b=a∧b
a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] \boldsymbol{a}^{\wedge}=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix} a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
-
用外积表示旋转
右手法则,四根手指由a指向b,拇指方向为旋转向量的方向,大小由两个向量的夹角决定
坐标系的欧氏变换
设定一个不动的惯性坐标系(世界坐标系w),相机或机器人是移动坐标系c,由一个矩阵T描述两个坐标系之间的转换关系、移动坐标系的运动、两个坐标系下同一点坐标的换算关系。
一个欧氏变换由平移和旋转两部分组成。
-
描述一个向量需要先规定一组基,再在这组基下用一组坐标描述,即按照坐标所述的方式用基向量组合出该向量。在不同的基下,应当有不同的描述,但组合的结果应该相同,都是同一个向量。
由此有
[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] \begin{bmatrix}\boldsymbol{e_1,e_2,e_3}\end{bmatrix}\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=\begin{bmatrix}\boldsymbol{e_1^{'},e_2^{'},e_3^{'}}\end{bmatrix}\begin{bmatrix}a^{'}_1\\a_2^{'}\\a_3^{'}\end{bmatrix} [e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤ -
同时左乘 [ e 1 T e 2 T e 3 T ] \begin{bmatrix}e_1^{T}\\e_2^{T}\\e_3^{T}\end{bmatrix} ⎣⎡e1Te2Te3T⎦⎤ ,左边变成单位矩阵,右边得到旋转矩阵
[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] ≜ R a ′ \begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=\begin{bmatrix}e_1^{T}e_1^{'}&e_1^{T}e_2^{'}&e_1^{T}e_3^{'}\\e_2^{T}e_1^{'}&e_2^{T}e_2^{'}&e_2^{T}e_3^{'}\\e_3^{T}e_1^{'}&e_3^{T}e_2^{'}&e_3^{T}e_3^{'}\end{bmatrix}\begin{bmatrix}a_1^{'}\\a_2^{'}\\a_3^{'}\end{bmatrix}\triangleq\boldsymbol{Ra^{'}} ⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′
旋转矩阵R由两组基之间的内积组成,一个旋转对应一个旋转矩阵。
-
性质:旋转矩阵是一个行列式为1的正交矩阵,并且行列式为1的正交矩阵是一个旋转矩阵。
正交阵: R T = R − 1 \boldsymbol{R^{T}}=\boldsymbol{R^{-1}} RT=R−1
其转置(逆)刻画了一个相反的旋转。
-
完整的运动由旋转加平移构成
a ′ = R a + t \boldsymbol{a^{'}=Ra+t} a′=Ra+t
变换矩阵与齐次坐标
由于
a
′
=
R
a
+
t
\boldsymbol{a^{'}=Ra+t}
a′=Ra+t的描述方式不符合线性运算,于是引入齐次坐标,和变换矩阵
[
a
′
1
]
=
[
R
t
0
T
1
]
[
a
1
]
≜
T
[
a
1
]
\begin{bmatrix}\boldsymbol{a^{'}}\\1\end{bmatrix}=\begin{bmatrix}\boldsymbol{R}&\boldsymbol{t}\\\boldsymbol{0^{T}}&1\end{bmatrix}\begin{bmatrix}\boldsymbol{a}\\1\end{bmatrix}\triangleq\boldsymbol{T}\begin{bmatrix}\boldsymbol{a}\\1\end{bmatrix}
[a′1]=[R0Tt1][a1]≜T[a1]
其中四维向量的最后一位常数表示缩放。
eg:
[
1
,
1
,
1
,
1
]
T
\begin{bmatrix}1,1,1,1\end{bmatrix}^{T}
[1,1,1,1]T和
[
2
,
2
,
2
,
2
]
T
\begin{bmatrix}2,2,2,2\end{bmatrix}^{T}
[2,2,2,2]T表示同一个点
变换矩阵T符合线性要求,有较好的性质:
b
~
=
T
1
a
~
,
c
~
=
T
2
b
~
⇒
c
~
=
T
2
T
1
a
~
\boldsymbol{\widetilde{b}}=\boldsymbol{T_1\widetilde{a}},\boldsymbol{\widetilde{c}}=\boldsymbol{T_2\widetilde{b}}\Rightarrow\boldsymbol{\widetilde{c}}=\boldsymbol{T_2T_1\widetilde{a}}
b
=T1a
,c
=T2b
⇒c
=T2T1a
其逆表示反向的变换
T
−
1
=
[
R
T
−
R
T
t
0
T
1
]
\boldsymbol{T^{-1}}=\begin{bmatrix}\boldsymbol{R^{T}}&\boldsymbol{-R^{T}t}\\\boldsymbol{0^{T}}&1\end{bmatrix}
T−1=[RT0T−RTt1]
旋转向量与欧拉角
用矩阵表示旋转,不够紧凑,矩阵9个值,但仅有三个自由度,冗余浪费。
任意一个旋转都可以用一个旋转轴和一个旋转角来刻画,于是用一个方向与转轴一致,大小与转角一致的向量来描述旋转,该向量称为旋转向量。
⭐️旋转向量到旋转矩阵的过程由罗德格里斯公式给出
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
∧
\boldsymbol{R}=cos\theta\boldsymbol{I}+(1-cos\theta)\boldsymbol{nn^{T}}+sin\theta\boldsymbol{n}^{\wedge}
R=cosθI+(1−cosθ)nnT+sinθn∧
⭐️由旋转矩阵到旋转向量
θ
=
a
r
c
c
o
s
(
t
r
(
R
)
−
1
2
)
\theta=arccos(\frac{tr(\boldsymbol{R})-1}{2})
θ=arccos(2tr(R)−1)
由于旋转轴在旋转后不变,因此转轴
n
\boldsymbol{n}
n是矩阵
R
\boldsymbol{R}
R特征值1对应的特征向量
R
n
=
n
\boldsymbol{Rn=n}
Rn=n
欧拉角
把一次旋转分解成三次绕不同轴的旋转,分解方式有很多种,定义也有很多种。最常见的一种定义方式为“偏航 俯仰 滚转”
1.物体绕z轴旋转,得到偏航角yaw
2.物体绕旋转后的Y轴旋转,得到俯仰角pitch
3.物体绕旋转后的X轴旋转,得到滚转交roll
写成向量用
[
r
,
p
,
y
]
T
[r,p,y]^{T}
[r,p,y]T来描述
欧拉角会遇到万向锁问题,当第二次绕Y轴旋转为90°时,第一次和第三次旋转会使用同一个轴,成为奇异性问题。
很少在程序中直接使用欧拉角,常用于人机交互界面,便于理解。
四元数
复数的扩展,紧凑,无奇异性
定义:
四元数是形如
q
=
s
+
x
i
+
y
j
+
z
k
=
[
s
,
v
]
q=s+xi+yj+zk=[s,\boldsymbol{v}]
q=s+xi+yj+zk=[s,v]的表达式,其中a,b,c,d均为实数,而i,j,k满足如下运算规则
× \times × | 1 | i | j | k |
---|---|---|---|---|
1 | 1 | i | j | k |
i | i | -1 | k | -j |
j | j | -k | -1 | i |
k | k | j | -i | -1 |
共轭:对于q=a+bi+cj+dk,其共轭为q*=a-bi-cj-dk
模长: ∣ ∣ q ∣ ∣ = q q ∗ = q ∗ q = a 2 + b 2 + c 2 + d 2 ||q||=\sqrt{qq*}=\sqrt{q*q}=\sqrt{a^2+b^2+c^2+d^2} ∣∣q∣∣=qq∗=q∗q=a2+b2+c2+d2
逆: q − 1 = 1 ∣ ∣ q ∣ ∣ q ∗ q^{-1}=\frac{1}{||q||}q* q−1=∣∣q∣∣1q∗
加减:实部虚部分别加减
q
a
±
q
b
=
[
s
a
±
s
b
,
v
a
±
v
b
]
\boldsymbol{q_a}\pm\boldsymbol{q_b}=[s_a\pm s_b,\boldsymbol{v_a\pm v_b}]
qa±qb=[sa±sb,va±vb]
乘法(叉乘):各相分别相乘再相加
写成向量形式为:
q
a
q
b
=
[
s
a
s
b
−
v
a
T
v
b
,
s
a
v
b
+
s
b
v
A
+
v
a
×
v
b
]
\boldsymbol{q_aq_b}=[s_as_b-\boldsymbol{v_a^{T}v_b},s_a\boldsymbol{v_b}+s_b\boldsymbol{v_A}+\boldsymbol{v_a\times v_b}]
qaqb=[sasb−vaTvb,savb+sbvA+va×vb]
点乘: q 1 ⋅ q 2 = s a s b + x a x b i + y a y b j + z a z b k \boldsymbol{q_1\cdot q_2}=s_as_b+x_ax_bi+y_ay_bj+z_az_bk q1⋅q2=sasb+xaxbi+yaybj+zazbk
四元数表示旋转
假设一个绕单位向量
n
=
[
n
x
,
n
y
,
n
z
]
\boldsymbol{n}=[n_x,n_y,n_z]
n=[nx,ny,nz]进行角度为
θ
\theta
θ的旋转,则表示这个旋转的四元数为:
q
=
[
c
o
s
θ
2
,
s
i
n
θ
2
(
n
x
,
n
y
,
n
z
)
]
\boldsymbol{q}=[cos\frac{\theta}{2},sin\frac{\theta}{2}(n_x,n_y,n_z)]
q=[cos2θ,sin2θ(nx,ny,nz)]
对于一个四元数
q
\boldsymbol{q}
q所表示的旋转,其转轴和转角为:
KaTeX parse error: Unknown column alignment: * at position 23: …{\begin{array}{*̲*lr**}\theta=2a…
对三维空间点
p
=
[
x
,
y
,
z
]
\boldsymbol{p}=[x,y,z]
p=[x,y,z]进行一个轴为
n
\boldsymbol{n}
n角为
θ
\theta
θ的旋转:
首先将三维空间点
p
\boldsymbol{p}
p用一个虚四元数描述:
p
=
[
0
,
x
,
y
,
z
]
=
[
0
,
v
]
\boldsymbol{p}=[0,x,y,z]=[0,\boldsymbol{v}]
p=[0,x,y,z]=[0,v]
用四元数表示旋转为:
q
=
[
c
o
s
θ
2
,
n
s
i
n
θ
2
]
\boldsymbol{q}=[cos\frac{\theta}{2},\boldsymbol{n}sin\frac{\theta}{2}]
q=[cos2θ,nsin2θ]
则旋转后的点为:
p
′
=
q
p
q
−
1
\boldsymbol{p^{'}}=\boldsymbol{qpq^{-1}}
p′=qpq−1
四元数与旋转矩阵的转化
相似 放射 射影变换
相似
允许物体进行均匀缩放,立方体经过相似变换依然为立方体,但大小发生了变化。
矩阵表示为:
T
s
=
[
s
R
t
0
T
1
]
\boldsymbol{T_s}=\begin{bmatrix}s\boldsymbol{R}&\boldsymbol{t}\\ \boldsymbol{0}^{T}&1\end{bmatrix}
Ts=[sR0Tt1]
放射变换
立方体经过放射变换后,不再是方的,但各个面仍然是平行四边形。
矩阵表示为:
T
A
=
[
A
t
0
T
1
]
\boldsymbol{T_A}=\begin{bmatrix}\boldsymbol{A}&\boldsymbol{t}\\ \boldsymbol{0}^{T}&1\end{bmatrix}
TA=[A0Tt1]
其中
A
\boldsymbol{A}
A只要求是可逆矩阵,不要求是正交矩阵。
射影变换
真实世界到相机照片的变换是一个射影变换,其矩阵形式为
T
P
=
[
A
t
a
T
v
]
\boldsymbol{T_P}=\begin{bmatrix}\boldsymbol{A}&\boldsymbol{t}\\ \boldsymbol{a}^{T}&v\end{bmatrix}
TP=[AaTtv]
左上角为可逆矩阵
A
\boldsymbol{A}
A,右上为平移
t
\boldsymbol{t}
t,左下缩放
a
T
\boldsymbol{a}^{T}
aT