目录

文章目录

• 目录
• 第1讲 引言
• 第2讲 初识SLAM
• 第3讲 三维空间刚体运动
• • 旋转矩阵
  • • 点，向量和坐标系
    • 坐标系间的欧式变换
    • 变换矩阵与齐次坐标
    • 齐次坐标(Homogeneous Coordinate)的优势
  • 旋转向量和欧拉角
  • • 旋转向量
    • 欧拉角
  • 四元数
  • • 四元数定义
    • 用单位四元数表示旋转

第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讲 三维空间刚体运动

旋转矩阵

点，向量和坐标系

1. 向量 α \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​]⊤

2. 向量的内积: 描述向量间的投影关系
   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∑3​ai​bi​=∣a∣∣b∣cos⟨a,b⟩

3. 向量的外积: 描述向量的旋转
   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=⎣⎡​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[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] a∧=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​

坐标系间的欧式变换

1. 欧式变换:在欧式变换前后的两个坐标系下,同一个向量的模长和方向不发生改变,是为欧式变换.一个欧式变换由一个旋转和一个平移组成。
2. 旋转矩阵 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​]⎣⎡​a1​a2​a3​​⎦⎤​=[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} ⎣⎡​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 } 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 )描述了一个相反的旋转。

3. 欧式变换的向量表示
   世界坐标系中的向量 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

变换矩阵与齐次坐标

1. 变换矩阵 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​]=[R0​t1​][a1​]≜T[a1​]
   齐次坐标的意义在于将欧式变换表示为线性关系。

2. 变换矩阵 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=[R0​t1​]∈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)的优势

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 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

2. 方便表示线线交点和点点共线
   在齐次坐标下,
   性质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​

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=(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), 这表示无穷远点.

5. 能够简洁的表示变换
   使用齐次坐标, 可以将加法运算转化为乘法运算。
   

旋转向量和欧拉角

旋转向量

1. 旋转矩阵的缺点:
   1. 旋转矩阵有9个量,但一次旋转只有3个自由度,这种表达方式是冗余的。
   2. 旋转矩阵自带约束(必须是行列式为1的正交矩阵),这些约束会给估计和优化带来困难。
2. 旋转向量：任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向表示旋转轴而长度表示旋转角.这种向量称为旋转向量(或轴角，Axis-Angle)。

假设有一个旋转轴为 n n n,角度为 θ \theta θ的旋转,其对应的旋转向量为 θ n \theta n θn。

3. 旋转向量和旋转矩阵之间的转换:

设旋转向量 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 对应的特征向量

欧拉角

1. 欧拉角将一次旋转分解成3个分离的转角.常用的一种ZYX转角将任意旋转分解成以下3个轴上的转角：
   • 绕物体的Z ZZ轴旋转,得到偏航角yaw
   • 绕旋转之后的Y YY轴旋转,得到俯仰角pitch
   • 绕旋转之后的X XX轴旋转,得到滚转角roll
2. 欧拉角的一个重大缺点是万向锁问题(奇异性问题):在俯仰角为90° 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度(由3次旋转变成了2次旋转)。

四元数

为什么需要四元数：对于三维旋转，找不到不带奇异性的三维向量描述方式。因此引入四元数。四元数是一种扩展的复数，既是紧凑的,也没有奇异性。

四元数定义

1. 四元数的定义
   一个四元数 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​+q1​i+q2​j+q3​k
   其中 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 为四元数的虚部。有实四元数和虚四元数的概念。

2. 四元数与旋转角度的关系:

   1. 在二维情况下, 任意一个旋转都可以用单位复数来描述, 乘 i i i 就是绕 i i i 轴旋转 9 0 ∘ 90^{\circ} 90∘ 。
   2. 在三维情况下, 任意一个旋转都可以用单位四元数来描述,乘 i i i 就是绕 i i i 轴旋转 18 0 ∘ 180^{\circ} 180∘ 。

3. 单位四元数和旋转向量之间的转换:
   设单位四元数 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 θ 的旋转。

   1. 从旋转向量到单位四元数:
      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θ​),nx​sin(2θ​),ny​sin(2θ​),nz​sin(2θ​)]T
   2. 从单位四元数到旋转向量:
      { θ = 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 表达旋转?

1. 把三维空间点用一个虚四元数 p p p 表示:
   p = [ 0 , x , y , z ] = [ 0 , v ] p=[0, x, y, z]=[0, v] p=[0,x,y,z]=[0,v]
2. 把旋转用单位四元数 q q q 表示:
   q = [ cos ⁡ θ 2 , n sin ⁡ θ 2 ] q=\left[\cos \frac{\theta}{2}, n \sin \frac{\theta}{2}\right] q=[cos2θ​,nsin2θ​]
3. 旋转后的点 p ′ p^{\prime} p′ 可表示为:
   p ′ = q p q − 1 p^{\prime}=q p q^{-1} p′=qpq−1

注意：只有单位四元数才能表示旋转，因此在程序中创建四元数后，记得调用normalize()将其单位化。