一、问题
- 我在什么地方?——定位。
- 周围环境是什么样?——建图。
二、视觉slam框架
` 图1 视觉SLAM流程图
- 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果在机器人中,还可能有码盘、惯性传感器等信息的读取和同步。
- 视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动,以及局部地图的样子。VO 又称为前端(Front End)。
- 后端优化(Optimization)。后端接受不同时刻视觉里程计测量的相机位姿,以及回环检测的信息,对它们进行优化,得到全局一致的轨迹和地图。由于接在 VO 之后,又称为后端(Back End)。
- 回环检测(Loop Closing)。回环检测判断机器人是否曾经到达过先前的位置。如果检测到回环,它会把信息提供给后端进行处理。
- 建图(Mapping)。它根据估计的轨迹,建立与任务要求对应的地图。
三、数学描述
- 什么是运动?我们要考虑从
k
−
1
k -1
k−1 时刻到
k
k
k 时刻,小萝卜的位置
x
x
x 是如何变化的。
数学模型:
其中 x k − 1 x_{k-1} xk−1是上一时刻的位置, u k u_k uk 是运动传感器的读数(有时也叫输入), w k w_k wk 为噪声。通过一个一般函数 f f f 来描述这个过程,而不具体指明 f f f 的作用方式。这使得整个函数可以指代任意的运动传感器,成为一个通用的方程,而不必限定于某个特殊的传感器上。我们把它称为运动方程。 - 什么是观测?假设小萝卜在 k 时刻,于
x
k
x_k
xk 处探测到了某一个路标
y
j
y_j
yj。
数学模型:
其中 z k , j z_{k,j} zk,j表示的是观测数据, y j y_j yj是观测到的路标, x k x_k xk是当前位置, v k , j v_{k,j} vk,j是观测的噪音。用一个抽象的函数 h h h 来描述这个过程,称之为观测方程。
四、参数化
假设小萝卜在平面中运动,那么,它的位姿由两个位置和一个转角来描述,即
x
k
=
[
x
,
y
,
θ
]
k
T
x_k = [x, y, θ] _k^T
xk=[x,y,θ]kT 。同时,运动传感器能够测量到小萝卜在每两个时间间隔位置和转角的变化量
u
k
=
[
∆
x
,
∆
y
,
∆
θ
]
k
T
u_k = [∆x, ∆y, ∆θ]^T_k
uk=[∆x,∆y,∆θ]kT ,那么,此时运动方程就可以具体化为:
我们知道激光传感器观测一个 2D 路标点时,能够测到两个量:路标点与小萝卜本体之间的距离
r
r
r 和夹角
ϕ
ϕ
ϕ。我们记路标点为
y
=
[
p
x
,
p
y
]
T
y = [p_x, p_y]^T
y=[px,py]T,观测数据为
z
=
[
r
,
ϕ
]
T
z = [r, ϕ]^T
z=[r,ϕ]T,那么观测方程就具体化为:
五、总结
针对不同的传感器,运动方程与观测方程有不同的参数化形式。如果我们保持通用性,把
它们取成通用的抽象形式,那么 SLAM 过程可总结为两个基本方程:
这两个方程描述了最基本的 SLAM 问题:当我们知道运动测量的读数
u
k
u_k
uk,以及传感器
的读数
z
k
,
j
z_{k,j}
zk,j 时,如何求解定位问题(估计
x
k
x_k
xk)和建图问题(估计
y
j
y_j
yj)。
Linux实践:
1、helloSLAM.cpp
#include <iostream>
using namespace std;
int main(int argc, char** argv)
{
cout<<"Hello SLAM!"<<endl;
return 0;
}
2、编译
g++ helloSLAM.cpp
注:如果你安装的Linux系统没有安装g++编译器,请使用如下命令进行安装
sudo apt-get install g++
3、运行
./a.out
4、结果
Hello SLAM!