SLAM算法-因子图建模

惊鸿一博

已于 2022-09-21 15:51:54 修改

阅读量2.1k

点赞数 2

分类专栏： SLAM 数学

于 2022-09-20 08:13:38 首次发布

原文链接：https://blog.csdn.net/u013721848/article/details/123448449

版权

SLAM 同时被 2 个专栏收录

225 篇文章 136 订阅 ¥29.90 ¥99.00

订阅专栏

超级会员免费看

数学

36 篇文章 9 订阅

订阅专栏

本文深入探讨了SLAM中的因子图模型，详细解释了概率图模型，包括贝叶斯网络、马尔科夫随机场和因子图。重点讨论了因子图在SLAM问题中的应用，以及如何通过线性化非线性模型进行推断。文章还概述了MAP推理和消元算法在解决推理问题中的作用。

摘要由CSDN通过智能技术生成

1 整体介绍

SLAM或者计算机视觉的问题目标是利用观察到的数据(图像,点云)来推理现实世界中的某些事情(全局状态),比如通过连续的视频帧推理相机的运动，比如通过3D点云数据推理机器人的移动状态(位置和姿态)。SLAM和计算机视觉都尝试描述一个用来解决此类问题的数学框架模型。解决此类问题需要三个要素:

模型: 在数学上将数据和全局状态关联起来
推理: 根据数据，利用模型计算全局状态的后验概率
学习: 利用成对样本和状态，来学习模型参数

1.0 概率图模型

因子图属于概率图模型(PGM)中的一种，先介绍PGM知识。主要包括三部分：表示，推理，学习。

在这里插入图片描述

1.0.1 表示

图是由节点和边构成的抽象网络图，其中包括几个元素：

(1) 点：表示随机变量
(2) 边：表示节点之间的关系,边具有方向
(3) 图分为: 有向图和无向图

在概率论部分,其中条件独立性是概率推断的基础，基本公式：

(1) 独立性: P(AB) =P(A)P(B)
(2) 条件独立性：P(AB|C) = P(A|C)P(B|C)
(2) 贝叶斯公式： f(A|B)=f(B|A)*f(A)/f(B)
(3) 联合概率乘法公式 = 条件概率 * 边缘概率： f(A,B) = f(A|B) * f(B)
(4) 条件概率分布变换规则: f(a,b|c) = f(a|b,c) * f(b|c)
- f(a,b) = f(a|b) * f(b), 然后都加上条件 c
(5) *联合分布链式法则：f(a,b,c) = f(a)f(b|c)f(c|a,b)

按照具体分类包括三类：贝叶斯网络，马尔科夫随机场，因子图。

1.0.2 贝叶斯网络

有向图

贝叶斯网络是个有向无圈图，由变量节点和相邻节点之间的有向边构成，其中边用父节点指向子节点的带箭头线段表示，用条件概率表达依赖关系，用先验概率表达没有父节点的节点。联合概率分布，可以表示为各个节点的条件概率分布的乘积：
在这里插入图片描述

按照贝叶斯乘积公式，看具体分解：

在这里插入图片描述

贝叶斯网络联合概率可以分解为乘积的形式主要是条件独立性,顺着箭头方向表示因果推理，逆着箭头方向表示证明推理。

1.0.3 马尔科夫随机场

无向图

利用势函数的乘积表示变量的概率分布：
在这里插入图片描述

势函数是关于随机变量的函数，随机变量取不同的值，势函数的大小不一样。当所有的势函数值都最大时，联合概率的取值最大。为了将势函数取值从正值扩充到任意实数值，将联合概率分布改写为吉布斯分布：

在这里插入图片描述

1.0.4 因子图

因子图属于无向图，因子指的是势函数，我们知道变量的联合概率可以表示为势函数的连乘积形式，将势函数表示乘图模型，势函数是关于变量的函数，变量用圆圈表示，势函数用方块表示，将之间的关联关系用线段连接形成的图为因子图：

定义在因子图上的概率分布，可以表示为各个因子的联乘积形式。可以理解为因子图是通过局部变量(圆圈节点)的依赖关系来表示整体的联合概率分布。

1.0.5 典型图模型

SLAM中或者计算机视觉中常用的模型有下面两类：隐马尔科夫模型和马尔科夫树模型。
在这里插入图片描述

我们知道模型包含两类：

(1) 判别模型：建立在数据p(w|x)上的全局状态可能性模型
(2) 生成模型：建立在全局状态p(x|w)上的数据可能性模型

以上图模型有个共同特点：含有从状态量w到测量数据x的有向边，该边表明w与x的关系：p(x|w) （表示在状态w下，观测数据x发生的概率），在给定的数据上构建了生成模型的概率分布。

上述模型的内部是非常稀疏的：每个数据量x只与一个状态量w连接，每个状态量与另外少量的几个状态量连接，结果是模型中蕴含大量的条件独立性关系，利用这种冗余性构建用于学习和推理的高效算法。

1.1 推理问题

概率图模型的推理是求解模型，推理复杂度高，而精确推理是NP难得问题，推理是和具体应用相关的需要先掌握基本算法，再看是如何在SLAM中解决问题。主要掌握几个推理算法，如变量消元算法。

1.1.1 MAP推理

在链式和树模型上可以动态规划进行：
在这里插入图片描述

这里不详细介绍动态规划，可以去看相关书籍。

备注：链式模型中的MAP推理公式，用到了联合概率乘积公式：p(w,x) = p(w|x) * p(x) $\propto$ p(w|x)

树模型中的MAP推理用到了贝叶斯公式：p(w|x) = p(x|w)*p(w) / p(x) $\propto$ p(x|w)*p(w)

1.1.2 消元算法

消元法计算边缘概率
基本思想是从联合概率分布中消除变量，求解边缘分布。边缘概率原始定义如下：
在这里插入图片描述

例1 下表描述ABC的联合概率，求A的边缘概率：

在这里插入图片描述

第一步，去掉B变量后，剩余的A和C的相同的概率做累加，得到AC概率表
第二步，去掉C后，相同的A做累加，得到关于A的表
第三步，归一化处理

主要采用消除变量，然后求和边缘化。

例2 链式模型，求边缘
在这里插入图片描述

（上面推导过程中有些错误，但是方法是对的）

1.1.3 时间复杂度

消元的时间复杂度影响因素：中间产生的变量因子的数量。
在这里插入图片描述

2 SLAM中的因子图

以上大量篇幅介绍了图模型和简单的推理算法，是通用的理论框架，那么怎么应用在SLAM领域了，可以带着这几个问题：
(1) SLAM中包含怎么样的图模型，贝叶斯有向图？因子图？MRF马尔科夫随机场？
(2) SLAM中的推理问题包含那些了，MAP推理？边缘化？
(3) SLAM推理的算法和消元算法有什么关系？时间复杂度是什么？

2.1 问题模型和表示

SLAM是在贝叶斯框架下的概率推断，主要目标是利用传感器的观测数据(z)，对机器人自身的位姿(变量x)和已知的控制输入进行推断，同时建立环境地图(变量l)。

在这里插入图片描述

贝叶斯框架

按照图的概率定义，联合概率密度函数表示为：
在这里插入图片描述

图模型的结构说明了数据之间关联，在具体应用中，可指定概率密度函数的具体形式，如高斯分布。这样可得到观测概率模型和运动概率模型，得到联合概率分布，而求解的方式一般采用模拟采样。实际我们并不关心联合概率分布和其分解，在SLAM领域最关心的是给定观测数据Z的条件下，最大化未知变量x和l，下面用大写X表示(x,l) ，转化为数学问题就是求未知状态的MAP估计，

在这里插入图片描述

参考：

求解的方法是贝叶斯转化。因为最终生成的观测似然函数不一定服从高斯分布，推断方法会无效。
因子图可以解决以上的问题，观测量Z不是最终要求解的对象，可以使用因子节点（表示变量之间的关系）去表示每一个后验概率密度P(X|Z)，每一个贝叶斯网络节点分解为相应的因子图中的变量节点和因子节点，因子与变量节点相连。