视觉SLAM笔记（4） SLAM的数学表述
视觉SLAM的核心在于用数学语言描述机器人在未知环境中运动并构建地图的过程。首先，通过离散时刻t=1,…,K记录机器人位置xk和路标yj的位置信息；其次，在每个时刻k，“小萝卜”利用传感器测量自身位置的变化与路标的观测数据；运动方程由函数f(x{k-1},uk,wk)描述位置变化；观测方程由函数h(yj,xk,vk,j)描述路标检测；参数化方法根据传感器类型（如激光雷达）不同而异；最后将SLAM问题建模为状态估计问题，并通过卡尔曼滤波器、扩展卡尔曼滤波器或图优化等方法进行求解以估计状态变量和地图结构。
视觉SLAM笔记（4） SLAM的数学表述
- 1. 数学表述
- 2. 运动方程
- 3. 观测方程
- 4. 参数化
- 5. 状态估计
1. 数学表述
在前面部分中，我们已经对SLAM技术中的各个模块及其主要功能形成了基本认识。然而，在实际应用中仅凭这种直观印象是无法编写能够正常运行的有效程序
为了使SLAM过程能够得到系统的理性刻画,我们需要将其转化为数学语言. 假设'小萝卜'正在携带某种传感器在未知环境中移动,请问如何用数学语言对此现象进行描述?
首先，在相机通常在特定时间段内捕获数据的情况下，则也仅关注这些关键时间点上的位置信息以及相应的地理信息。这种转化使得我们可以将一段连续的时间运动转化为一系列离散的时间点（t=1,…,K）上所发生的事件。在每一个这样的关键时间点上，则以变量x表示“小萝卜”自身的所在位置，在这种情况下，则各个关键时间点上的位置分别用x_1,…,x_K表示，并完整地描绘出了“小萝卜”的整体移动轨迹。地图方面，则假设地图是由许多个 路标 （Landmark）组成的结构，并且相应地，在每个关键时间点上使用的传感器能够测得部分路标点及其观测数据。
2. 运动方程
不妨设路标点一共有 N 个，用 y1,…,yN 表示它们
在这样设定中，“小萝卜”携带着传感器在环境中运动，由如下两件事情描述：
- 运动的本质是什么？我们需要关注的是，在连续两个时间点k−1和k之间，“小萝卜”的位置坐标x发生了怎样的变化情况。
- 观测的本质是什么？我们可以假设，在时间点k时，“小萝卜”位于位置坐标xk处，并通过探测手段检测到了某个路标yj。
通常情况下, 机器人会装备一个用于监测自身运动状态的传感器. 例如, 在工业领域常见的有编码盘或惯性式传感器. 此外, 这个传感器能够获取与运动相关的数据. 然而, 并非所有的读数都直接反映了位置的变化量. 实际上, 它也可能包含加速度、角速度等数据.
然而，在任何类型的传感器面前都存在一个统一适用的基本数学框架：对于每个时间点k来说，
x_k = f(x_{k-1}, u_k, w_k)
其中uk表示运动相关参数（也可视为输入量），而wk则代表测量过程中的噪声或不确定性。
值得注意的是，在本研究中采用一个通用函数 f 来描述该过程，并无需明确其作用机制。
从而使得整个数学模型成为一种普适性的方程式，并且能够适用于多种不同的运动传感器类型。
我们将其命名为运动方程。
3. 观测方程
与运动方程对应的一个是另一个观测方程。 观测方程具体说明了这样的过程：当"小萝卜"处于位置x_k时观察到路标点y_j后产生了数据z_{k,j}。 同样地，在这种情况下我们使用一个抽象的函数h来建立这种关系：即有公式h(y_j,x_k,v_{k,j})= z_{k,j}成立。 其中的v_{k,j}代表了这次测量中的噪声
随着观测中使用的传感器类型更加多样化，在本研究中涉及的观测数据 z 和观测方程 h 多呈现多样化的形式
4. 参数化
或许觉得所采用的 f 和 h 函数仅仅停留在表面描述层面？它们究竟如何具体地关联着运动与观测？与此同时，在这里 x, y, z 等变量又各自代表了什么？
基于机器人动态行为的真实轨迹和不同类型的传感器，在实际应用中往往需要采用多种参数化形式（Parameterization）来进行建模与控制设计。例如，在二维平面中运行的 robotic agent 可以通过其位置坐标增量来描述其位姿（pose），即由位置坐标 x_k = [x, y]^T 和方位角 \theta_k 组成的状态向量来表征其当前位置与朝向；同时该机器人能够通过速度传感器或其他类型传感器测量其位置坐标增量 u_k = [\Delta x, \Delta y, \Delta \theta]^T 从而实现对动态行为的实时跟踪与预测。在此基础上建立的运动学模型也就可以相应地表示为这些状态变量随时间变化的关系式：
这是简单的线性关系
不过，并不是所有的传感器都直接能测量出位移和角度变化
所以也存在着其他形式更加复杂的运动方程，那时可能需要进行动力学分析
关于观测方程：
比方说“小萝卜”携带着一个二维激光传感器
当知道激光传感器观测一个 2D 路标点时，能够测到两个量：
- 路标点与小萝卜本体之间的距离 r
- 路标点与小萝卜本体之间的夹角 ϕ
记路标点为 y = [ px，py ]T（为保持简洁，省略了下标）
观测数据为 z = [r，ϕ]T
那么观测方程就具体化为：
在视觉 SLAM 中使用的是相机作为传感器的一种。那么观测方程描述的是路标点经过相机拍摄后生成图像像素的过程。直观上可以看出，在不同类型的传感器下这两个模型有不同的参数化形式。如果希望保持一般性，则可将它们表示为通用的抽象形式。这样就形成了基本的两步方程框架。
这些方程构成了SLAM问题的基础：在获得运动测量结果u以及传感器反馈z的情况下，请问如何通过估计变量x来实现位置确定以及通过估计变量z来构建环境模型？
5. 状态估计
此时将该SLAM问题建模为一个状态估计问题，并通过带有观测噪声的数据来推断内部的状态变量
状态估计问题的求解取决于两个方程的表达式以及系统的噪声统计特性。基于运动和观测方程的线性属性以及系统噪声的统计特性进行分类分析。主要包含以下两种类型：一是运动和观测均呈现线性特性的系统；二是运动或观测中存在非线性因素影响的系统。
最简单的线性高斯系统（Linear Gaussian, LG 系统）是
其估计值可通过无偏且最优的卡尔南滤波算法（Kalman Filter, KF）获得
在复杂非线性非高斯系统（Non-Linear Non-Gaussian, NLG系统）中
在关键点处对系统进行线性化处理，并采用预测-更新两大步骤进行求解
最早实现的实时视觉 SLAM 系统是基于 EKF 开发的
随后为了解决 EKF 的局限性（如线性化带来的误差以及高斯噪声假设），
人们开始采用粒子滤波器（Particle Filter）等其他滤波方案，并进一步发展出非线性优化方法。
当下, 当前主流的视觉 SLAM 方法采用基于图优化(Graph Optimization)的技术完成对状态的估计过程
研究表明，在计算资源得到充分满足的情况下，实践表明优化方法倾向于采用。
参考：
相关推荐：
视觉SLAM系列（3） 视觉 SL AM 体系架构
视觉 SL AM 系列（2） 相机系统
视觉 SL AM 系列（1） 基础入门
谢谢！