开篇提问
如何描述刚体在三维空间中的运动？
3.1旋转矩阵
矩阵可以用来表示点、向量、坐标系、平移、旋转及变换，还可以表示坐标系中的物体和其他运动部件
3.1.1 点、向量、坐标系
空间点的表示 P = a x i + b y j + c z k P=a_x { \bf i}+b_y { \bf j }+c_z { \bf k} P=axi+byj+czk
对于向量来说，如果向量起始于原点则可以表示为
P
=
a
x
i
+
b
y
j
+
c
z
k
{\bf P}=a_x { \bf i}+b_y { \bf j }+c_z { \bf k}
P=axi+byj+czk
在计算机图像学中，通过增加一个比例因子，可以对图像进行比例变换操作，由于该比例因子可以增加或减少所有向量的长短，这样就可以无须重新绘图而很用以地改变向量的尺寸
P
=
[
P
x
P
y
P
z
]
,
a
x
=
P
x
w
,
b
y
=
P
y
w
,
c
z
=
P
z
w
,
{\bf P} =\begin{bmatrix} P_x \\ P_y \\P_z\\ \end{bmatrix} ,a_x =\frac {P_x}{w},b_y =\frac {P_y}{w},c_z =\frac {P_z}{w},
P=⎣⎡PxPyPz⎦⎤,ax=wPx,by=wPy,cz=wPz,
对于两个向量a b,通常内积表示两个向量间的投影关系
a
⋅
b
=
a
T
b
=
∑
i
=
1
3
a
i
b
i
=
∣
a
∣
∣
b
∣
c
o
s
⟨
a
,
b
⟩
\bf a \cdot b = a^Tb = \rm \sum_{i=1}^3 a_i b_i =\bf |a||b| \rm cos \bf \langle a,b \rangle
a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,b⟩
其中
c
o
s
⟨
a
,
b
⟩
cos\langle a,b \rangle
cos⟨a,b⟩是指向量a,b之间的夹角
对于外积的运算得到的是垂直于这两个向量的向量
a
×
b
=
∣
e
1
e
2
e
3
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
=
\bf a \times b = \begin{vmatrix}e_1 & e_2 & e_3 \\a_1 &a_2 &a_3\\b_1 &b_2 &b_3 \end{vmatrix} =\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}b=
a×b=∣∣∣∣∣∣e1a1b1e2a2b2e3a3b3∣∣∣∣∣∣=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b= a^b
a^ 事实上是一个反对称矩阵，任何向量都唯一对应一个反对称矩阵，我们将外积写成了矩阵的乘法，把他变成了线性变化
3.1.2 坐标系间的欧式变换
在机器人的运动学中，我们需要对机器人建立两个坐标系，一个是固定不动的世界坐标系，另一个是伴随机器人的移动坐标系。两个坐标系之间的运动我们可以分解为由一个旋转和平移组成，这种运动称之为刚体运动，而者之间的转换我们称之为欧氏变换。
旋转矩阵R：刻画了旋转前后同一个向量的坐标变换关系
[
a
1
a
2
a
3
]
=
R
a
′
\begin{bmatrix} a_1\\a_2\\a_3\\\end{bmatrix} = \bf R\rm a'
⎣⎡a1a2a3⎦⎤=Ra′
同时旋转矩阵自身还有一些性质及他是行列式为1的正交矩阵
特殊正交群
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
\text {特殊正交群}SO(n)=\lbrace \bf R \rm \in R^{n\times n} \mid \bf RR^T = I, \rm det(\bf R)=\rm1\rbrace
特殊正交群SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
在欧式变换中，旋转和平移的合成为
a
′
=
R
a
+
t
\bf a' =Ra+t
a′=Ra+t,其中t为平移向量
3.1.3 变换矩阵与其次变换
向量的旋转与平移可以从上小节中进行解算，但谁对于多组的欧氏变换通过一个一个的推算是相当复杂的，这是我们通过引入齐次坐标和变换矩阵使之可以方便的进行线性计算
[
a
′
1
]
=
[
R
t
0
T
1
]
[
a
1
]
=
T
[
a
1
]
\begin{bmatrix} \bf a' \\ 1 \end{bmatrix} = \begin{bmatrix} \bf R & \bf t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} \bf a \\ 1 \end{bmatrix}=\bf T \begin{bmatrix} \bf a \\ 1 \end{bmatrix}
[a′1]=[R0Tt1][a1]=T[a1]
我们称T为变换矩阵，对于多次的欧式变换
c
=
T
2
T
1
a
\bf c = T_2T_1a
c=T2T1a即可得到
3.3 旋转向量和欧拉角
3.3.1 旋转向量
旋转向量的提出是出于对上节中旋转矩阵R的计算冗余和他的自身约束条件（正交矩阵）的限制而提出的一种紧凑型描述方式：旋转轴n&旋转角
θ
\theta
θ
旋转向量与旋转矩阵之间的转换为
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
{\bf R}=cos\theta{\bf I} +(1-cos\theta) {\bf n n^T}+sin\theta \bf n
R=cosθI+(1−cosθ)nnT+sinθn^
n^为反对称矩阵转换符，同样旋转矩阵也可以转换旋转向量这里就不作介绍。
3.3.2 欧拉角
欧拉角的提出使为了使人们可以直观的理解坐标轴的旋转运动，将三个轴的旋转分为偏航角yaw、俯仰角pitch、翻滚角roll
万向锁问题，当我们的俯仰角为
±
9
0
。
\pm90^。
±90。时，会出现第一次旋转与第三次旋转使用的是同一个轴，使系统丢失了一个自由度（即我两个自由度就可以完成这次旋转）----奇异性问题----导致欧拉角不适用于插值和迭代
NOTE:在Slam中我们不会使用欧拉角来表示旋转。
3.4 四元数
优点：紧凑的，没有奇异性
缺点：不直观，运算复杂
三维旋转可以由单位四元数来描述，一个四元数拥有一个实部三个虚部
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
{\bf q} = q_0 + q_1i +q_2j +q_3k
q=q0+q1i+q2j+q3k
四元数的运算需要遵循一下法则
{
i
2
=
j
2
=
k
2
=
−
1
i
j
=
k
,
j
i
=
−
k
j
k
=
i
,
k
j
=
−
i
k
i
=
j
,
i
k
=
−
j
\begin {cases}i^2 = j^2 = k^2 = -1\\ ij=k,ji=-k\\jk=i,kj=-i \\ ki =j,ik=-j \end{cases}
⎩⎪⎪⎪⎨⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
对于四元数的模长我们可以把它想象成复数类型，
∥
q
∥
=
s
a
2
+
x
a
2
+
y
a
2
+
z
a
2
\|q\|=\sqrt{s_a^2+x^2_a+y_a^2+z_a^2}
∥q∥=sa2+xa2+ya2+za2
对于其他四元数的运算性质可以在实际应用中进行掌握即可。
用四元数表示旋转，我们假设用一点p=[x,y,z] 由一个单位四元数q指定的旋转，经过旋转之后得到p’，使用矩阵描述是 p’= Rp
使用四元数的话,三维空间点用一个虚四元数描述
p
=
[
0
,
x
,
y
,
z
]
T
=
[
0
,
v
]
T
{\bf p} = [0,x,y,z]^T = [0,v]^T
p=[0,x,y,z]T=[0,v]T
p
′
=
q
p
q
−
1
\bf p' = qpq^{-1}
p′=qpq−1
最后把
p
′
\bf p'
p′的虚部取出，即取得旋转之后点的坐标
结尾点题
刚体在三维空间中通过位置与姿态进行表述，其运动可以看为矩阵的旋转与平移的结合，我们可以通过旋转矩阵、旋转向量、欧拉角以及四元数来对旋转进行表示，由于旋转矩阵的冗余性，旋转向量与欧拉角的奇异性，最终采用四元数来表征刚体在三维空间中的运动。
扫一扫
2770
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
