Karto SLAM 源码分析 （三） Levenberg-Marquardt 算法

  本文想讲了一下在实际过程中优化的原因，后面尝试去理解 KARTO 算法中 LM的应用，后面还附带 LM的推导的相关博文，希望能在本文中对 LM 算法在 KARTO 中有一个大概的认识。
目录：

Levenberg-Marquardt 算法简介

为什么要进行最优化

      在 karto_slam 中使用了 LM 的最优化算法。最优化顾名思义就是寻找最优的解。 在建图过程中，我们将激光扫描的结果作为一种限定条件，我们希望通过这些限定条件得到机器人的准确位姿，这些位姿信息包括 机器人的速度，位移，旋转角。而LM 算法就是通过 激光数据 来找到机器人最可能在的位置，而寻找的过程使用到的算法就是最优化算法。

 上图中红色为激光测得的扫描到的障碍物信息，黑色点是机器人实际的位姿。理想情况，知道周围的环境信息（障碍物）就应该知道自己的位置，但是事实上激光数据的误差，里程计的误差等会对直接的判断产生影响，这就需进行选择最可能的位姿，假设灰色点是机器人可能的位姿， 我们需要选取最优，单个点的优化还很好理解。

 同样的对于一副很大的地图，如上图中 70m × 150m 大小，路径上每隔0.5s 进行一次激光数据的记录，完成这样的一幅图会有 5000+ 的节点数据和对应的位置判定，而当前未知的判定又紧紧与之前判断联系，稳定可靠的建图需要 稳定高效的优化算法实现。

最优化算法的基本原理

梯度下降

  LM算法是介于牛顿法与梯度下降法之间的一种非线性优化方法，对于过参数化问题不敏感，能有效处理冗余参数问题，使代价函数陷入局部极小值的机会大大减小。
关于详细的算法这篇文章讲解的很全面，英文资料原文：http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf， 以下是截取部分重要的点做简要介绍，系统的学习最好阅读这篇文献。

所有非线性优化都是通过一系列迭代实现：

这里主要介绍梯度下降法，每一次的迭代主要包含了两个部分

1. 寻早最快下降的方向 Hd
2. 找到最快下降方向上的下降步长值 F-value

寻找下降最快的方向通过对函数求导来实现，而下降步长需要与导数的值相关联，求导值越大说明变化剧烈，此时离极值点较远，步长可设置的大一些，而导数越小可以理解为离极值点越近，应该减小步长，防止移动太多，步长的合理设置可以帮我们尽快的找到极值

LM 算法与改进的Guass-Newton法

 在LM算法中，每次迭代是寻找一个合适的阻尼因子λ，当λ很小时，算法就变成了GAuss-Newton法的最优步长计算式，λ很大时，蜕化为梯度下降法的最优步长计算式。

其中：

1. || f(x) || 是对 f(x) 做最小二乘法

Levenberg-Marquardt迭代（LM算法）-改进Guass-Newton法 https://www.cnblogs.com/monoSLAM/p/5249339.html

优化过程与评价函数

 评价函数是判断对 位姿 p 估计的好坏， 那是如何来评价的呢？
 我们知道我们使用的是栅格地图，当我们对一个节点的位姿进行了变化，那么此节点对应的激光的位置也进行了变化，那么全局栅格占有情况就出现了变化，连续的几个节点确定了此处的局部地图，也就是说储存了相关区域的节点信息，那么将新估计位姿后的栅格占有图和邻近节点的栅格图做一个匹配。
 对于每一个栅格来说，只有命中和不命中两种两种状况，如果节点的位姿完全正确的话，其栅格占有情况和局部地图的栅格占有情况应该是完全一致的；统计出每一个栅格的命中情况，这样就能得到一个评价，评价越高即栅格情况的命中率越高，推断位姿的占有情况与局部地图符合的越好，这也为每一个参数的优化指明了方向。

参考资料:
Levenberg-Marquardt迭代（LM算法）-改进Guass-Newton法 https://www.cnblogs.com/monoSLAM/p/5249339.html
英文资料原文：http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf

2018年8月2日 Master Plan Day 1