因子图的理论基础

因子图是在机器人领域slam相关的重要数学工具. 今天结合董靖在深蓝的演讲讨论一下

如何描述因子图的问题

上图表示一个slam模型,其中,odometry 信息可以通过轮速计或者imu获得.红色表示机器人对路标点的观测,绿色表示机器人对自身运动的观测.

如果用贝叶斯网络来描述,蓝色是路标节点的变量, 黄色表示机器人本身的状态信息,而红色和绿色表示观测变量.

X表示状态量,Z表示系统观测量,通常在一个系统中,由极少的状态变量来决定一个观测变量,比如机器人某一时刻的位姿和路标信息就可以决定一个特征点的观测信息,而机器人的相对运动则取决于机器人在连续两个时刻的位姿状态.

我们把这个叫做生成模型,我们假设知道状态,从而推测观测的情况. 而这和我们在slam中想做的事情是相反的,我们想做的是已知观测,去求状态变量. 而因子图和贝叶斯网络关系比较紧密.

因子图对应的inference model是为了求解状态变量(圆形),而方框对应观测变量,或者对应因子,一般因子图还会包括先验因子来存在系统的先验值,来确保数值可解.

目标: 估算系统最有可能的状态.

为什么要用指数函数的形式,我们假设系统概率分布是高斯分布, 然后, 误差越小,对应的phi越大. 所以,系统状态和观测值吻合, 因子增大,最后的乘积越大. 我们希望找到状态变量,去吻合观测变量.

使得指数最大化,就等于使得最小二乘的结果最小. 因子图问题,变为了非线性最小二乘问题.

给定一个初始值,我们可以通过迭代,得到增量,得到更好的值, 那么,我该如何得到增量呢?方法是,我们要求雅克比,目标是使得函数最小化,这时候,通过雅克比,问题变成了一个线性最小问题.

求解线性最小二乘,我们有比较成熟的办法. 比如高斯牛顿法.=>求normal equation,或者直接求解

如果JtJ是正定阵,我们可以将其分解为RtR,其中R是上三角阵.我们就可以通过Cholesky分解的方式得到求解.除了使用normal equation,我们还可以直接求解

这里用到的数学工具QR分解.但是它比cholesky比较慢,不过cholesky数值稳定性较差.

来看一下jacobian矩阵,他具有很强的稀疏性, 同时,状态量的排序会影响最后的效率,

如果因子图只修改了一小部分,那么大部分其实都没变. 为了增加效率,需要实现增量推理.