从贝叶斯推断到SLAM的数学模型
从贝叶斯推断到SLAM的数学模型
- 贝叶斯推断
- SLAM数学问题简单描述
- 滤波方式
- 非线性优化方式
SLAM的全称是Simultaneous Location And Mapping，即同时定位与建图。机器人要在未知的环境中实时的估算自己的运动、定位并建图。 想象一个移动机器人在未知环境中移动，使用位于机器人上的传感器对一些未知的地标进行观察，就如下图所示。
要描述SLAM问题，还得从贝叶斯推断说起。
贝叶斯推断
贝叶斯定理，实际上是从条件概率衍生而来的。所以我们先从条件概率公式说起。
首先我们要知道三个概念：
- 条件概率，表示事件A在事件B发生的条件下发生的概率，用符号 P ( A ∣ B ) P(A|B) P(A∣B)表示；
- 联合概率，表示两个事件共同发生的概率，可表示为 P ( A ∩ B ) P(A\cap B) P(A∩B) 或者 P ( A , B ) P(A,B) P(A,B)、 P ( A B ) P(AB) P(AB)；
- 边缘概率，表示某个事件发生的概率，事件A的边缘概率可表示为 P ( A ) P(A) P(A)，事件B的边缘概率可表示为 P ( B ) P(B) P(B)
设 A 与 B 为样本空间 Ω 中的两个事件，其中 P(B)>0。那么在事件 B 发生的条件下，事件 A 发生的条件概率为：
P ( A ∣ B ) = P ( A ∩ B ) P ( B ) P(A|B)=\frac{P(A\cap B)}{P(B)} P(A∣B)=P(B)P(A∩B)
用文氏图表达：
通过条件概率公式，有：
P ( A ∩ B ) = P ( A ) P ( B ∣ A ) = P ( B ) P ( A ∣ B ) P(A\cap B)=P(A)P(B|A)=P(B)P(A|B) P(A∩B)=P(A)P(B∣A)=P(B)P(A∣B)
故得到贝叶斯公式：
P ( A ∣ B ) = P ( B ∣ A ) P ( A ) P ( B ) P(A|B)=\frac{P(B|A)P(A)}{P(B)} P(A∣B)=P(B)P(B∣A)P(A)
贝叶斯定理（英语：Bayes’ theorem）是概率论中的一个定理，描述在已知一些条件下，某事件的发生概率。比如，如果已知某癌症与寿命有关，使用贝叶斯定理则可以通过得知某人年龄，来更加准确地计算出他罹患癌症的概率。
我们可以把各项称之为：
P ( A ∣ B ) P(A|B) P(A∣B)：后验概率
P ( A ) P(A) P(A)：先验概率
P ( B ∣ A ) P(B|A) P(B∣A)：似然率
P ( b ) P(b) P(b)：边缘似然率
一般来说，直接求后验概率会很难求，那么怎么办呢？我们可以求得右侧的概率来推断后验概率是多少。
举个例子，隔壁班有个妹子经常看见你就对你笑，你很喜欢这个妹子，但又担心表白失败，作为工科生，肯定要理智一点，想判断她是否喜欢你，那么产生了一个后验概率： P ( 妹 子 喜 欢 你 ∣ 妹 子 经 常 对 你 笑 ) P(妹子喜欢你|妹子经常对你笑) P(妹子喜欢你∣妹子经常对你笑)
直接得出这个概率是很难的，这个时候你需要通过贝叶斯公式：
P ( 妹 子 喜 欢 你 ∣ 妹 子 经 常 对 你 笑 ) = P ( 妹 子 喜 欢 你 ) × P ( 妹 子 经 常 对 你 笑 ∣ 妹 子 喜 欢 你 ) P ( 妹 子 经 常 对 你 笑 ) P(妹子喜欢你|妹子经常对你笑) =P(妹子喜欢你) × \frac{P(妹子经常对你笑|妹子喜欢你)}{P(妹子经常对你笑)} P(妹子喜欢你∣妹子经常对你笑)=P(妹子喜欢你)×P(妹子经常对你笑)P(妹子经常对你笑∣妹子喜欢你)
假设妹子喜欢你的概率 P ( 妹 子 喜 欢 你 ) P(妹子喜欢你) P(妹子喜欢你)是50%。
这时候你还不能表白，因为这个表白的话很容易被拒绝，这样就没朋友做了。这时候你就想，她天天对我笑，那她对其他人是不是也一直笑呢？于是你就偷偷打听，终于，你打听到了她很少对别人笑，这个打听到的信息即 P ( 妹 子 经 常 对 你 笑 ∣ 妹 子 喜 欢 你 ) P ( 妹 子 经 常 对 你 笑 ) \frac{P(妹子经常对你笑|妹子喜欢你)}{P(妹子经常对你笑)} P(妹子经常对你笑)P(妹子经常对你笑∣妹子喜欢你)，我们又把这个称为可能性函数，假设这个数值为1.8。
那么， P ( 妹 子 喜 欢 你 ∣ 妹 子 经 常 对 你 笑 ) = P ( 妹 子 喜 欢 你 ) × P ( 妹 子 经 常 对 你 笑 ∣ 妹 子 喜 欢 你 ) P ( 妹 子 经 常 对 你 笑 ) = 0.5 × 1.8 = 0.9 P(妹子喜欢你|妹子经常对你笑) =P(妹子喜欢你) × \frac{P(妹子经常对你笑|妹子喜欢你)}{P(妹子经常对你笑)}=0.5×1.8=0.9 P(妹子喜欢你∣妹子经常对你笑)=P(妹子喜欢你)×P(妹子经常对你笑)P(妹子经常对你笑∣妹子喜欢你)=0.5×1.8=0.9
妹子在经常对你笑的前提下，喜欢你的概率是90%，还不快去表白？
贝叶斯推断的本质是求解某事的条件概率，我们可以把先验信息看成是旧的看法，可能性函数看成是新的信息，后验概率看成是新的看法。
有同学可能要问了……这个可能性函数是要怎么来的？说实话，我也在这里想了很久。假设我们要解决的是机器学习中的分类问题（朴素贝叶斯），那么我们往往会掌握一些数据，而可能性函数就是通过这些数据来计算的，朴素贝叶斯的核心就是求解贝叶斯公式右侧的所有概率，进而分类出最有可能发生的事件。
贝叶斯推断和朴素贝叶斯的区别在于，贝叶斯推断只是单纯推断某件事发生的概率，朴素贝叶斯是把所有可能的事件概率计算出来，取概率最大的当成分类的结果。
比如一共有 P ( 喜 欢 你 ∣ 对 你 笑 ) P(喜欢你|对你笑) P(喜欢你∣对你笑)和 P ( 不 喜 欢 你 ∣ 对 你 笑 ) P(不喜欢你|对你笑) P(不喜欢你∣对你笑)两种后验概率，贝叶斯推断是计算其中一种后验概率发生的概率，而朴素贝叶斯是把两种都算了，然后返回概率大的事件给你，在上述的例子中，朴素贝叶斯有种决策的效果，它告诉你应该去表白。好了，貌似扯远了……
关于朴素贝叶斯可看此处
那这样我们就介绍完贝叶斯推断了。
SLAM数学问题简单描述
在SLAM问题上，可以用贝叶斯框架来进行描述（不然也不会费这么多话来解释贝叶斯推断了）。在SLAM中，我们可以为相机（或机器人）定义两个变量，一个是状态量，一个是观测量。分别用以下字母来进行表示：
状态变量： x 1 : k = ( x 1 , x 2 … … , x k ) x_{1:k}=(x_1,x_2……,x_k) x1:k=(x1,x2……,xk)
观测量： z 1 : k = ( z 1 , z 2 … … , z k ) z_{1:k}=(z_1,z_2……,z_k) z1:k=(z1,z2……,zk)
其中，状态量指的是每个时刻下相机的姿态、位置速度和三维点坐标， x k x_k xk专指 k k k时刻的状态。
而观测量指的是在当前状态下所看到的特征点的像素坐标，同样， z k z_k zk专指 k k k时刻的观测。
我们的任务是把相机的状态估计出来，这是一个状态估计问题，因此，我们可以将SLAM问题转化为求解后验概率的问题：
P ( x 1 : k ∣ z 1 : k , x 0 ) P(x_{1:k} | z_{1:k},x_0) P(x1:k∣z1:k,x0)
刚刚贝叶斯推断时已经说了，直接求后验概率比较困难，但是求一个状态最优估计，使得在该状态下，后验概率最大化（Maximize a Posterior，MAP）是可行的：
x M A P ∗ = a r g m a x P ( x 1 : k ∣ z 1 : k , x 0 ) = a r g m a x P ( z 1 : k , x 0 ∣ x 1 : k ) P ( x 1 : k ) x^*_{MAP}=argmaxP(x_{1:k}|z_{1:k},x_0)=argmaxP(z_{1:k},x_0|x_{1:k})P(x_{1:k}) xMAP∗=argmaxP(x1:k∣z1:k,x0)=argmaxP(z1:k,x0∣x1:k)P(x1:k)
由于贝叶斯公式中右侧的分母与 x x x状态无关，所以使后验概率最大化无需考虑其分母。
这里最大化后验概率意思就是最可能的状态是什么。
但由于我们一般只关心当前时刻的状态，所以又可以写成：
x M A P ∗ = a r g m a x P ( x k ∣ z 1 : k , x 0 ) = a r g m a x P ( z 1 : k , x 0 ∣ x k ) P ( x k ) x^*_{MAP}=argmaxP(x_k|z_{1:k},x_0)=argmaxP(z_{1:k},x_0|x_{k})P(x_{k}) xMAP∗=argmaxP(xk∣z1:k,x0)=argmaxP(z1:k,x0∣xk)P(xk)
三句话概括SLAM问题：
SLAM问题是状态的估计问题，就是说我们要估计相机或空间点的状态。那怎么估计呢？
理论上要求的：在这个观测值的前提下，相机的状态最有可能是怎样的？
实际上求的：在这个相机状态下，最可能观测到什么。
可以看到从概率学角度来看，SLAM问题本质上是求使用贝叶斯公式求最大后验概率。
关于求解SLAM的问题，世间产生了两种较为主流的方式。
滤波方式
回到上述所说，，我们要求的是： P ( x 1 : k ∣ z 1 : k , x 0 ) P(x_{1:k}|z_{1:k},x_0) P(x1:k∣z1:k,x0)
由于我们只关心当前时刻的状态，又可写为： P ( x k ∣ z 1 : k , x 0 ) = ∭ . . . ∫ P ( x 1 : k ∣ z 1 : k , x 0 ) d x 1 d x 2 d x 3 . . . d x k − 1 P(x_k|z_{1:k},x_0)=\iiint...\int P(x_{1:k}|z_{1:k},x_0)dx_1dx_2dx_3...dx_{k-1} P(xk∣z1:k,x0)=∭...∫P(x1:k∣z1:k,x0)dx1dx2dx3...dxk−1
这是一个边缘化的过程，相当于把之前的状态信息当成一个先验信息融合进来，把时刻1到k-1时刻的状态都marginalization掉。
但是这样会产生一个问题，计算量很大，于是我们可以在马尔科夫假设下，用迭代贝叶斯滤波去求解。
马尔卡夫的假设下， P ( x k ∣ x 1 : k − 1 ) = P ( x k ∣ x k − 1 ) P(x_k|x_{1:k-1})=P(x_k|x_{k-1}) P(xk∣x1:k−1)=P(xk∣xk−1)
根据贝叶斯公式： P ( x k ∣ z 1 : k , x 0 ) = P ( z k , x k ∣ z 1 : k − 1 , x 0 ) ∫ P ( z k , x k ∣ z 1 : k − 1 , x 0 ) d x k P(x_k|z_{1:k},x_0)=\frac{P(z_k,x_k|z_{1:k-1},x_0)}{\int P(z_k,x_k|z_{1:k-1},x_0)dx_k} P(xk∣z1:k,x0)=∫P(zk,xk∣z1:k−1,x0)dxkP(zk,xk∣z1:k−1,x0)
由于分子是一个归一化常量，我们用不着管它。
于是又可以写成：
P ( x k ∣ z 1 : k , x 0 ) = P ( z k , x k ∣ z 1 : k − 1 , x 0 ) ∫ P ( z k , x k ∣ z 1 : k − 1 , x 0 ) d x k ∝ P ( z k , x k ∣ z 1 : k − 1 ) = P ( z k ∣ x k ) P ( x k ∣ z 1 : k − 1 , x 0 ) = P ( z k ∣ x k ) ∫ x k − 1 P ( x k − 1 ∣ z 1 : k , x 0 ) d x k − 1 = P ( z k ∣ x k ) ∫ x k − 1 P ( x k ∣ x k − 1 ) P ( x k − 1 ∣ z 1 : k − 1 , x 0 ) d x k − 1 P(x_k|z_{1:k},x_0)=\frac{P(z_k,x_k|z_{1:k-1},x_0)}{\int P(z_k,x_k|z_{1:k-1},x_0)dx_k} \propto P(z_k,x_k|z_{1:k-1}) \\ =P(z_k|x_k)P(x_k|z_{1:k-1},x_0)=P(z_k|x_k)\int_{x_{k-1}}P(x_{k-1}|z_{1:k},x_0)dx_{k-1} \\ = P(z_k|x_k)\int_{x_{k-1}}P(x_k|x_{k-1})P(x_{k-1}|z_{1:k-1},x_0)dx_{k-1} P(xk∣z1:k,x0)=∫P(zk,xk∣z1:k−1,x0)dxkP(zk,xk∣z1:k−1,x0)∝P(zk,xk∣z1:k−1)=P(zk∣xk)P(xk∣z1:k−1,x0)=P(zk∣xk)∫xk−1P(xk−1∣z1:k,x0)dxk−1=P(zk∣xk)∫xk−1P(xk∣xk−1)P(xk−1∣z1:k−1,x0)dxk−1
P ( z k ∣ x k ) P(z_k|x_k) P(zk∣xk)表示观测模型
P ( x k ∣ x k − 1 ) P(x_k|x_{k-1}) P(xk∣xk−1)表示预测模型
P ( x k − 1 ∣ z 1 : k − 1 , x 0 ) P(x_{k-1}|z_{1:k-1},x_0) P(xk−1∣z1:k−1,x0)表示上一时刻的状态分布
上述的式子就是贝叶斯滤波的公式。
贝叶斯滤波步骤总结：
第一步预测：从上一时刻状态分布预测当前时刻状态分布：
P ( x k ∣ z 1 : k − 1 , x 0 ) = ∫ x k − 1 P ( x k ∣ x k − 1 ) P ( x k ∣ z 1 : k − 1 , x 0 ) d x k − 1 P(x_k|z_{1:k-1},x_0)=\int_{x_{k-1}}P(x_k|x_{k-1})P(x_k|z_{1:k-1},x_0)dx_{k-1} P(xk∣z1:k−1,x0)=∫xk−1P(xk∣xk−1)P(xk∣z1:k−1,x0)dxk−1
P ( x k ∣ z 1 : k − 1 , x 0 ) P(x_k|z_{1:k-1},x_0) P(xk∣z1:k−1,x0)表示预测值。
第二步更新：使用当前时刻观测量更新后验概率：
P ( x k ∣ z 1 : k , x 0 ) ∝ P ( z k ∣ x k ) P ( x k ∣ z 1 : k − 1 , x 0 ) P(x_k|z_{1:k},x_0) \propto P(z_k|x_k)P(x_k|z_{1:k-1},x_0) P(xk∣z1:k,x0)∝P(zk∣xk)P(xk∣z1:k−1,x0)
P ( x k ∣ z 1 : k , x 0 ) P(x_k|z_{1:k},x_0) P(xk∣z1:k,x0)表示后验概率
P ( z k ∣ x k ) P(z_k|x_k) P(zk∣xk) 表示观测值
P ( x k ∣ z 1 : k − 1 , x 0 ) P(x_k|z_{1:k-1},x_0) P(xk∣z1:k−1,x0)表示预测值。
反复迭代。
有小伙子可能想问了，贝叶斯滤波和卡尔曼滤波有什么关系？卡尔曼滤波是贝叶斯滤波的特例，当状态分布、预测模型和观测模型均服从高斯分布，那么这样的滤波称为卡尔曼滤波。
为什么高斯分布会和SLAM扯上关系，就是因为有时候我们把这些模型假设成高斯分布。
关于卡尔曼滤波的工作方式可见：卡尔曼滤波
非线性优化方式
非线性优化方式的核心就在于批量优化和多次迭代。
与滤波方式不太一样的是，要求解： P ( x ∣ z 1 : k , x 0 ) P(x|z_{1:k},x_0) P(x∣z1:k,x0)，这里的状态x一般是多个时刻的，比如我们定义了很多关键帧，进行一次优化的时候是优化这多个时刻的状态，并且进行一次优化的时候会迭代很多次。
而滤波方式每一次优化只迭代一次。
下面直接放出我做的ppt好了- -，实在懒得敲latex。
可以看到我们把贝叶斯公式乘上一个-log，从而达到由最大化转最小化的作用。。
这时候，假设模型服从高斯分布。关于高斯分布的一些性质，可看此处：高斯分布
可以看到我们已经把SLAM问题转化成一个最小二乘问题了，接下来，我们使用某种非线性优化方法去解它，比如高斯牛顿法：
重复以上多次，直到增量接近于0，非线性优化就完成了，SLAM问题也就解决了。
参考文献：
https://zhuanlan.zhihu.com/p/81728490?utm_source=wechat_session
https://games-cn.org/games-webinar-20180426-43/
视觉SLAM十四讲
如果我的文章对你有帮助，欢迎关注，点赞，评论。