目录
文章目录
第1讲 引言
SLAM是Simultaneous Localization and Mapping的缩写，中文译作同时定位与地图构建。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。
习题答案汇总：链接
第2讲 初识SLAM
自主运动两大基本问题：我在什么地方？（定位）周围什么样子？（建图）
How to SLAM？——Sensors
两类传感器：
- 安装于环境中的：二维码，GPS，导轨、磁条
- 携带于机器人本体上：IMU，激光，相机
相机分类：
- 单目 Monocular，没有深度，必须通过移动相机产生深度moving view stereo
- 双目 Stereo，通过视差计算深度
- 深度 RGBD，通过物理方法测量深度
- 其他 鱼眼 全景 Event Camera，etc
整体视觉SLAM流程图如下：
- 前端：Visual Odometry
- 后端：optimization
- 回环检测 Loop Closing
- 建图 Mapping
视觉里程计VO：又称为前端
- 相邻图像估计相机运动
- 通过两张图像计算运动和结构
- 不可避免地有漂移
后端优化：
-从带有噪声的数据中优化轨迹和地图
- 最大后验概率MAP
- 前期EKF为代表，现在图优化为代表
回环检测：
- 检测机器人是否回到早先位置
- 识别到达过的场景
- 计算图像间相似性
建图：
- 用于导航、规划、通讯、可视化、交互等
- 度量地图VS拓扑地图
- 稀疏地图VS稠密地图
SLAM问题的数学表述：
小萝卜携带着传感器在环境中运动”，由如下两件事情描述:
1.什么是运动 ?我们要考虑从
k
−
1
k-1
k−1时刻到
k
k
k时刻，小萝卜的位置
x
x
x是如何变化的。
运动方程:
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
x_k = f(x_{k-1},u_k,w_k)
xk=f(xk−1,uk,wk)
- x k x_k xk， x k − 1 x_{k-1} xk−1 表示小萝卜在 k k k和 k − 1 k−1 k−1 时刻的位置
- u k u_k uk表示运动传感器的读数(有时也叫输入)
- w k w_k wk表示噪声
2.什么是观测 ?假设小萝卜在k时刻于
x
k
x_k
xk处探测到了某一个路标
y
j
y_j
yj，我们要考虑这件事情是如何用数学语言来描述的。
z
k
,
j
=
h
(
y
j
,
x
k
,
v
k
,
j
)
z_{k,j} = h(y_j,x_k,v_{k,j})
zk,j=h(yj,xk,vk,j)
- z k , j z_{k,j} zk,j表示小萝卜在 x k x_k xk位置上看到路标点 y j y_j yj产生的观测数据
- y j y_j yj表示第 j j j个路标点
- v k , j v_{k, j} vk,j表示噪声
这两个方程描述了最基本的SLAM问题：当知道运动测量的读数 u u u，以及传感器的读数 z z z时,如何求解定位问题(估计 x x x)和建图问题(估计 y y y)？这时，我们就把SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？
第3讲 三维空间刚体运动
旋转矩阵
点，向量和坐标系
-
向量 α \alpha α在线性空间的基 [ e 1 , e 2 , e 3 ] [e_{1},e_{2},e_{3}] [e1,e2,e3]下的坐标为 [ α 1 , α 2 , α 3 ] ⊤ [\alpha_{1},\alpha_{2},\alpha_{3}]^\top [α1,α2,α3]⊤
-
向量的内积: 描述向量间的投影关系
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_{i} b_{i}=|a||b| \cos \langle a, 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 a \times b=\left[\begin{array}{ccc} i & j & k \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{array}\right]=\left[\begin{array}{c} a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{1} \end{array}\right]=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] b \triangleq a^{\wedge} b a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤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[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right]
a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
坐标系间的欧式变换
- 欧式变换:在欧式变换前后的两个坐标系下,同一个向量的模长和方向不发生改变,是为欧式变换.一个欧式变换由一个旋转和一个平移组成。
- 旋转矩阵 R R R
-
旋转矩阵 R R R 的推导:
设单位正交基 [ e 1 , e 2 , e 3 ] \left[e_{1}, e_{2}, e_{3}\right] [e1,e2,e3] 经过一次旋转变成了 [ e 1 ′ , e 2 ′ , e 3 ′ ] \left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right] [e1′,e2′,e3′], 对于同一个向量 a a a, 在两个坐标系下的坐标分别为 [ a 1 , a 2 , a 3 ] T \left[a_{1}, a_{2}, a_{3}\right]^{T} [a1,a2,a3]T 和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T \left[a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime}\right]^{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 ′ ] \left[e_{1}, e_{2}, e_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right]\left[\begin{array}{l} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] [e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
等式左右两边同时左乘 [ e 1 T , e 2 T , e 3 T ] T \left[e_{1}^{T}, e_{2}^{T}, e_{3}^{T}\right]^{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[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{ccc} e_{1}^{T} e_{1}^{\prime} & e_{1}^{T} e_{2}^{\prime} & e_{1}^{T} e_{3}^{\prime} \\ e_{2}^{T} e_{1}^{\prime} & e_{2}^{T} e_{2}^{\prime} & e_{2}^{T} e_{3}^{\prime} \\ e_{3}^{T} e_{1}^{\prime} & e_{3}^{T} e_{2}^{\prime} & e_{3}^{T} e_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \triangleq R a^{\prime} ⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡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 } S O(n)=\left\{R \in \mathbb{R}^{n \times n} \mid R R^{T}=I, \operatorname{det}(R)=1\right\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1} -
旋转矩阵是正交矩阵(其转置等于其逆), 旋转矩阵的逆 R − 1 R^{-1} R−1 (即转置 R T R^{T} RT )描述了一个相反的旋转。
-
- 欧式变换的向量表示
世界坐标系中的向量 a a a, 经过一次旋转(用旋转矩阵 R R R 描述)和一次平移(用平移向量 t t t 描述)后,得到了 a ′ a^{\prime} a′ :
a ′ = R a + t a^{\prime}=R a+t a′=Ra+t
变换矩阵与齐次坐标
-
变换矩阵 T T T :
在三维向量的末尾添加 1 , 构成的四维向量称为齐次坐标将旋转和平移写入变换矩阵 T T T 中,得到:
[ a ′ 1 ] = [ R t 0 1 ] [ a 1 ] ≜ T [ a 1 ] \left[\begin{array}{c} a^{\prime} \\ 1 \end{array}\right]=\left[\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} a \\ 1 \end{array}\right] \triangleq T\left[\begin{array}{l} a \\ 1 \end{array}\right] [a′1]=[R0t1][a1]≜T[a1]
齐次坐标的意义在于将欧式变换表示为线性关系。 -
变换矩阵 T T T 的性质:
- 变换矩阵
T
T
T 构成特殊欧式群
S
E
S E
SE
S E ( 3 ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } S E(3)=\left\{T=\left[\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid R \in S O(3), t \in \mathbb{R}^{3}\right\} SE(3)={T=[R0t1]∈R4×4∣R∈SO(3),t∈R3} - 变换矩阵的逆表示一个反向的欧式变换
T − 1 = [ R T − R T t 0 1 ] T^{-1}=\left[\begin{array}{cc} R^{T} & -R^{T} t \\ 0 & 1 \end{array}\right] T−1=[RT0−RTt1]
齐次坐标(Homogeneous Coordinate)的优势
-
方便判断是否在直线或平面上
若点 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 a x+b y+c=[a, b, c]^{T} \cdot[x, y, 1]=l^{T} \cdot p^{\prime}=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 a x+b y+c z+d=[a, b, c, d]^{T} \cdot[x, y, z, 1]=A^{T} \cdot p^{\prime}=0 ax+by+cz+d=[a,b,c,d]T⋅[x,y,z,1]=AT⋅p′=0 -
方便表示线线交点和点点共线
在齐次坐标下,
性质1：可以用两个点 p p p， q q q的齐次坐标叉乘结果表示它们的共线 l l l
性质2：可以用两条直线 l l l， m m m的齐次坐标叉乘结果表示它们的交点 x x x
这里利用了叉乘的性质: 叉乘结果与两个运算向量都垂直。
- 性质1的证明:
l T ⋅ p = ( p × q ) ⋅ p = 0 l T ⋅ q = ( p × q ) ⋅ q = 0 \begin{aligned} &l^{T} \cdot p=(p \times q) \cdot p=0 \\ &l^{T} \cdot q=(p \times q) \cdot q=0 \end{aligned} 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 \begin{aligned} l^{T} \cdot p &=l^{T} \cdot(l \times m)=0 \\ m^{T} \cdot p &=m^{T} \cdot(l \times m)=0 \end{aligned} lT⋅pmT⋅p=lT⋅(l×m)=0=mT⋅(l×m)=0
- 能够区分向量和点
- 点 ( 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)
-
能够表达无穷远点
对于平行直线 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=(k b,-k a, 0) x=l×m=(kb,−ka,0), 将其转为非齐次坐标,得到 x = x= x= ( k b / 0 , − k a / 0 ) = ( inf , − inf ) (k b / 0,-k a / 0)=(\inf ,-\inf ) (kb/0,−ka/0)=(inf,−inf), 这表示无穷远点. -
能够简洁的表示变换
使用齐次坐标, 可以将加法运算转化为乘法运算。
旋转向量和欧拉角
旋转向量
- 旋转矩阵的缺点:
- 旋转矩阵有9个量,但一次旋转只有3个*度,这种表达方式是冗余的。
- 旋转矩阵自带约束(必须是行列式为1的正交矩阵),这些约束会给估计和优化带来困难。
- 旋转向量：任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向表示旋转轴而长度表示旋转角.这种向量称为旋转向量(或轴角，Axis-Angle)。
假设有一个旋转轴为 n n n,角度为 θ \theta θ的旋转,其对应的旋转向量为 θ n \theta n θn。
- 旋转向量和旋转矩阵之间的转换:
设旋转向量 R R R表示一个绕单位向量 n n n，角度为 θ \theta θ的旋转。
- 旋转向量到旋转矩阵:
R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ R=\cos \theta I+(1-\cos \theta) n n^{T}+\sin \theta n^{\wedge} R=cosθI+(1−cosθ)nnT+sinθn∧ - 旋转矩阵到旋转向量:
- 旋转角 θ = arccos ( tr ( R ) − 1 2 ) \theta=\arccos \left(\frac{\operatorname{tr}(R)-1}{2}\right) θ=arccos(2tr(R)−1)
- 旋转轴 n n n 是矩阵 R R R 特征值 1 对应的特征向量
欧拉角
- 欧拉角将一次旋转分解成3个分离的转角.常用的一种ZYX转角将任意旋转分解成以下3个轴上的转角：
- 绕物体的Z ZZ轴旋转,得到偏航角yaw
- 绕旋转之后的Y YY轴旋转,得到俯仰角pitch
- 绕旋转之后的X XX轴旋转,得到滚转角roll
- 欧拉角的一个重大缺点是万向锁问题(奇异性问题):在俯仰角为90° 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个*度(由3次旋转变成了2次旋转)。
四元数
为什么需要四元数：对于三维旋转，找不到不带奇异性的三维向量描述方式。因此引入四元数。四元数是一种扩展的复数，既是紧凑的,也没有奇异性。
四元数定义
-
四元数的定义
一个四元数 q q q 拥有一个实部和三个虚部
q = q 0 + q 1 i + q 2 j + q 3 k q=q_{0}+q_{1} i+q_{2} j+q_{3} k q=q0+q1i+q2j+q3k
其中 i , j , k i, j, k i,j,k, 为四元数的 3 个虚部,它们满足以下关系式(自己和自己的运算像复数,自己和别人的运算像叉乘):
{ 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 \left\{\begin{array}{l} 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 \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
也可以用一个标量和一个向量来表达四元数:
q = [ s , v ] , s = q 0 ∈ R v = [ q 1 , q 2 , q 3 ] T ∈ R 3 q=[s, v], \quad s=q_{0} \in \mathbb{R} \quad v=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3} q=[s,v],s=q0∈Rv=[q1,q2,q3]T∈R3
s s s 为四元数的实部, v v v 为四元数的虚部。有实四元数和虚四元数的概念。 -
四元数与旋转角度的关系:
- 在二维情况下, 任意一个旋转都可以用单位复数来描述, 乘 i i i 就是绕 i i i 轴旋转 9 0 ∘ 90^{\circ} 90∘ 。
- 在三维情况下, 任意一个旋转都可以用单位四元数来描述,乘 i i i 就是绕 i i i 轴旋转 18 0 ∘ 180^{\circ} 180∘ 。
-
单位四元数和旋转向量之间的转换:
设单位四元数 q q q 表示一个绕单位向量 n = [ n x , n y , n z ] T n=\left[n_{x}, n_{y}, n_{z}\right]^{T} n=[nx,ny,nz]T, 角度为 θ \theta θ 的旋转。- 从旋转向量到单位四元数:
q = [ cos ( θ 2 ) , n sin ( θ 2 ) ] T = [ cos ( θ 2 ) , n x sin ( θ 2 ) , n y sin ( θ 2 ) , n z sin ( θ 2 ) ] T q=\left[\cos \left(\frac{\theta}{2}\right), n \sin \left(\frac{\theta}{2}\right)\right]^{T}=\left[\cos \left(\frac{\theta}{2}\right), n_{x} \sin \left(\frac{\theta}{2}\right), n_{y} \sin \left(\frac{\theta}{2}\right), n_{z} \sin \left(\frac{\theta}{2}\right)\right]^{T} q=[cos(2θ),nsin(2θ)]T=[cos(2θ),nxsin(2θ),nysin(2θ),nzsin(2θ)]T - 从单位四元数到旋转向量:
{ θ = 2 arccos q 0 [ n x , n y , n z ] = [ q 1 , q 2 , q 3 ] T / sin θ 2 \left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}} \end{array}\right. {θ=2arccosq0[nx,ny,nz]=[q1,q2,q3]T/sin2θ
- 从旋转向量到单位四元数:
用单位四元数表示旋转
给定一个空间三维点 p = [ x , y , z ] ∈ R 3 p=[x, y, z] \in \mathbb{R}^{3} p=[x,y,z]∈R3,以及一个由轴角 n , θ n, \theta n,θ 指定的旋转,三维点 p p p 经过旋箦后变为 p ′ p^{\prime} p′ 。如何使用单位四元数 q q q 表达旋转?
- 把三维空间点用一个虚四元数
p
p
p 表示:
p = [ 0 , x , y , z ] = [ 0 , v ] p=[0, x, y, z]=[0, v] p=[0,x,y,z]=[0,v] - 把旋转用单位四元数
q
q
q 表示:
q = [ cos θ 2 , n sin θ 2 ] q=\left[\cos \frac{\theta}{2}, n \sin \frac{\theta}{2}\right] q=[cos2θ,nsin2θ] - 旋转后的点
p
′
p^{\prime}
p′ 可表示为:
p ′ = q p q − 1 p^{\prime}=q p q^{-1} p′=qpq−1
注意：只有单位四元数才能表示旋转，因此在程序中创建四元数后，记得调用normalize()将其单位化。