Slam基础 三维空间运动

三维空间刚体运动
旋转矩阵
点和向量,坐标系
向量 a a a在线性空间的基 [ e 1 , e 2 , e 3 ] [e_1, e_2, e_3] [e1​,e2​,e3​]下的坐标为 [ a 1 , a 2 , a 3 ] T [a_1, a_2, a_3]^T [a1​,a2​,a3​]T.

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 a = [e_1, e_2, e_3] \left[

a1a2a3

\right] = a_1e_1 + a_2e_2 + a_3e_3 a=[e1​,e2​,e3​]⎣⎡​a1​a2​a3​​⎦⎤​=a1​e1​+a2​e2​+a3​e3​

向量的内积与外积

向量的内积: 描述向量间的投影关系
a ⋅ b = a T b = ∑ i = 1 3 a i b i = ∣ a ∣   ∣ b ∣ cos ⁡ 〈 a , b 〉 a \cdot b = a^T b = \sum_{i=1}^3 a_ib_i = |a|,|b| \cos \langle a,b \rangle a⋅b=aTb=i=1∑3​ai​bi​=∣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 a \times b = \left[

ia1b1ja2b2ka3b3

\right] = \left[
a2b3−a3b2a3b1−a1b3a1b2−a2b1

\right] = \left[
0a3−a2−a30a1a2−a10

\right] b \triangleq a ^\wedge b a×b=⎣⎡​ia1​b1​​ja2​b2​​ka3​b3​​⎦⎤​=⎣⎡​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​⎦⎤​=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​b≜a∧b

其中 a ∧ a^\wedge a∧表示 a a a的反对称矩阵
a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] a ^\wedge = \left[

0a3−a2−a30a1a2−a10

\right] a∧=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​

坐标系间的欧氏变换
欧式变换:

在欧式变换前后的两个坐标系下,同一个向量的模长和方向不发生改变,是为欧式变换.

一个欧式变换由一个旋转和一个平移组成.

旋转矩阵 R R R:

旋转矩阵 R R R的推导:

设单位正交基 [ e 1 , e 2 , e 3 ] [e_1, e_2, e_3] [e1​,e2​,e3​]经过一次旋转变成了 [ e 1 ′ , e 2 ′ , e 3 ′ ] [e_1’, e_2’, e_3’] [e1′​,e2′​,e3′​],对于同一个向量 a a a,在两个坐标系下的坐标分别为 [ a 1 , a 2 , a 3 ] T [a_1, a_2, a_3]^T [a1​,a2​,a3​]T和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T [a_1’, a_2’, a_3’]^T [a1′​,a2′​,a3′​]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 ′ ] [e_1, e_2, e_3] \left[

a1a2a3

\right] = [e_1’, e_2’, e_3’] \left[
a′1a′2a′3

\right] [e1​,e2​,e3​]⎣⎡​a1​a2​a3​​⎦⎤​=[e1′​,e2′​,e3′​]⎣⎡​a1′​a2′​a3′​​⎦⎤​

等式左右两边同时左乘 [ e 1 T , e 2 T , e 3 T ] T [e_1^T, e_2^T, e_3^T]T [e1T​,e2T​,e3T​]T,得到
[ 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 ′ \left[

a1a2a3

\right] = \left[
eT1e′1eT2e′1eT3e′1eT1e′2eT2e′2eT3e′2eT1e′3eT2e′3eT3e′3

\right] \left[
a′1a′2a′3

\right] \triangleq R a’ ⎣⎡​a1​a2​a3​​⎦⎤​=⎣⎡​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​⎦⎤​⎣⎡​a1′​a2′​a3′​​⎦⎤​≜Ra′

矩阵 R R R描述了旋转,称为旋转矩阵.

旋转矩阵 R R R的性质

旋转矩阵是行列式为1的正交矩阵,任何行列式为1的正交矩阵也是一个旋转矩阵.所有旋转矩阵构成特殊正交群 S O SO SO:
S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } SO(n) = { R \in \mathbb{R}^{n \times n} | RR^T = I, \det®=1 } SO(n)={R∈Rn×n∣RRT=I,det®=1}

旋转矩阵是正交矩阵(其转置等于其逆),旋转矩阵的逆 R − 1 R^{-1} R−1(即转置 R T R^T RT)描述了一个相反的旋转.
欧式变换的向量表示:

世界坐标系中的向量 a a a,经过一次旋转(用旋转矩阵 R R R描述)和一次平移(用平移向量 t t t描述)后,得到了 a ′ a’ a′:
a ′ = R a + t a’ = Ra + t a′=Ra+t

变换矩阵与齐次坐标
变换矩阵 T T T:

在三维向量的末尾添加1,构成的四维向量称为齐次坐标.将旋转和平移写入变换矩阵 T T T中,得到:

[ a ′ 1 ] = [ R t 0 1 ] [ a 1 ] ≜ T [ a 1 ] \left[

a′1

\right] = \left[
R0t1

\right] \left[
a1

\right] \triangleq T \left[
a1

\right] [a′1​]=[R0​t1​][a1​]≜T[a1​]
齐次坐标的意义在于将欧式变换表示为线性关系.

变换矩阵 T T T的性质:

变换矩阵 T T T构成特殊欧式群 S E SE SE
S E ( 3 ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \left{ T = \left[

R0t1

\right] \in \mathbb{R}^{4\times4} | R \in SO(3), t \in \mathbb{R}^3 \right} SE(3)={T=[R0​t1​]∈R4×4∣R∈SO(3),t∈R3}

变换矩阵的逆表示一个反向的欧式变换
T − 1 = [ R T − R T t 0 1 ] T^{-1} = \left[

RT0−RTt1

\right] T−1=[RT0​−RTt1​]

齐次坐标(Homogeneous Coordinate)的优势
优势1:方便判断是否在直线或平面上

若点 p = ( x , y ) p=(x,y) p=(x,y)在直线 l = ( a , b , c ) l=(a,b,c) l=(a,b,c)上,则有:
a x + b y + c = [ a , b , c ] T ⋅ [ x , y , 1 ] = l T ⋅ p ′ = 0 ax+by+c = [a,b,c]^T \cdot [x,y,1] = l^T \cdot p’ = 0 ax+by+c=[a,b,c]T⋅[x,y,1]=lT⋅p′=0

若点 p = ( x , y , z ) p=(x,y,z) p=(x,y,z)在平面 A = ( a , b , c , d ) A=(a,b,c,d) A=(a,b,c,d)上,则有:
a x + b y + c z + d = [ a , b , c , d ] T ⋅ [ x , y , z , 1 ] = A T ⋅ p ′ = 0 ax+by+cz+d = [a,b,c,d]^T \cdot [x,y,z,1] = A^T \cdot p’ = 0 ax+by+cz+d=[a,b,c,d]T⋅[x,y,z,1]=AT⋅p′=0

优势2:方便表示线线交点和点点共线

在齐次坐标下,
可以用两个点 p p p, q q q的齐次坐标叉乘结果表示它们的共线 l l l.
可以用两条直线 l l l, m m m的齐次坐标叉乘结果表示它们的交点 x x x.

这里利用叉乘的性质: 叉乘结果与两个运算向量都垂直:

性质1的证明:
l T ⋅ p = ( p × q ) ⋅ p = 0 l T ⋅ q = ( p × q ) ⋅ q = 0 l^T \cdot p = (p \times q) \cdot p = 0 \ l^T \cdot q = (p \times q) \cdot q = 0 lT⋅p=(p×q)⋅p=0lT⋅q=(p×q)⋅q=0

性质2的证明:
l T ⋅ p = l T ⋅ ( l × m ) = 0 m T ⋅ p = m T ⋅ ( l × m ) = 0 l^T \cdot p = l^T \cdot (l \times m) = 0 \ m^T \cdot p = m^T \cdot (l \times m) = 0 lT⋅p=lT⋅(l×m)=0mT⋅p=mT⋅(l×m)=0

优势3:能够区分向量和点

点 ( x , y , z ) (x,y,z) (x,y,z)的齐次坐标为 ( x , y , z , 1 ) (x,y,z,1) (x,y,z,1)
向量 ( x , y , z ) (x,y,z) (x,y,z)的齐次坐标为 ( x , y , z , 0 ) (x,y,z,0) (x,y,z,0)
优势4:能够表达无穷远点

对于平行直线 l = ( a , b , c ) l=(a,b,c) l=(a,b,c)和 m = ( a , b , d ) m=(a,b,d) m=(a,b,d),求取其交点的齐次坐标 x = l × m = ( k b , − k a , 0 ) x=l \times m=(kb, -ka, 0) x=l×m=(kb,−ka,0),将其转为非齐次坐标,得到 x = ( k b / 0 , − k a / 0 ) = ( inf ⁡ , − inf ⁡ ) x = (kb/0, -ka/0) = (\inf, -\inf) x=(kb/0,−ka/0)=(inf,−inf),这表示无穷远点.

优势5:能够简洁的表示变换

使用齐次坐标,可以将加法运算转化为乘法运算.