浅析分支定界法在SLAM技术中的应用

• SLAM介绍
• Cartographer介绍
• 解释分支定界法
• 应用演示
• 反思总结

• 经过近30年的发展，SLAM技术日益成熟，最早应用于军事，如勇气号火星探测车。为了执行任务，借助它SLAM该技术在不能实时遥控的未知行星上进行导航和避障。到如今，SLAM该技术已广泛应用于民用场所，从家用扫地机到无人驾驶汽车SLAM技术，在AR及VR领域均有SLAM技术身影。

• 传感器数据：图像、深度、距离等

  • 在视觉SLAM中主要为相机图像信息的读取和预处理。如果是在机器人中，还可能有码盘、惯性传感器(IMU)等信息的读取和同步。
  • 上图就是我们所说的点云，它只能反映机器人所在环境中的一个部分。而预处理主要是对激光雷达原始数据进行优化，并将一些有问题的数据进行剔除，或进行滤波。

• 前端扫描匹配：通过读取相邻两帧激光传感器扫描信息之间的关系来估计自身的位姿。

  • 匹配是很关键的一步，它的好坏将会直接影响SLAM构建的地图精度，其主要是把当前在局部环境上的一个点云数据，在已经建立地图上寻找到对应的位置。与我们玩的拼图游戏有点相似，就是在已经拼好的画面中找到相似之处，确定新的拼图该放的位置。而在SLAM过程中，需要将激光雷达当前采集的点云（红色部分）匹配拼接到原有地图中

• 后端优化：用来减少扫描匹配后产生的累积误差，并利用地图构建模块生成环境地图信息。

  • 后端优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端（Back End）

  • 数据融合与简单的贴图是有很大差异的，因为外部环境不仅有静态的，还有动态的，此时，如果在机器人旁边闯入了一只小狗，实际在进程数据融合的过程会更为复杂，需要用到很多概率算法，且处理难度很大。

• 回环检测：通过检测当前位置的估计值和历史位置的 估计值是否相同，来消除累积误差，从而减少地图漂移的现象。

  • 回环检测（Loop Closing）。回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。

  • 举例来说，如果遇到回环问题，匹配算法不够优秀，或者在环境中存在很不巧的干扰，当机器人绕着环境走一圈后，便有可能出现原本应该闭合的一个环形走廊断开了。

    比如正常地图应该这样：

  • 在环境较大场景，回环问题是不得不面对的，但现实总是不理想，即使拥有了高精度的激光雷达传感器，也难免存在一定误差，而回环的难点在于，在一开始的少许误差并不会被发觉，直到机器人走完一圈后，随着误差的不断累积，导致了环路无法闭合，出现这种情况一般很难回天。

  • 以下是思岚科技员工在他们办公室进行的测试，当机器人已经绕场一周后，ROS构建的地图出现了中断，而利用SLAMWARE模块构建的地图是一个完美的闭环，它与思岚科技办公室的设计图完美重合。

• 地图构建：形成栅格地图、点云图

  • 它根据估计的轨迹，建立与任务要求对应的地图。

• 激光建图
• 导航避障
• 建图与导航避障

• 机器人(扫地机器人、市面上大部分能移动的机器人)

• 自动驾驶

  达摩院的快递车

• VR、AR

  定位、虚拟与现实的匹配

Cartographer_Google的论文

• SLAM目前是一种实时建立地图的有效方式，实时生成可视化平面图有助于操作者评估捕获数据的质量和覆盖范围。构建便携式捕获平台需要在有限的计算资源下操作。我们介绍了在我们的背包地图平台中使用的方法，它实现了实时地图绘制和5厘米分辨率的环路闭合。为了实现实时闭环检测，我们使用了一种分支定界方法来计算scan-to-submap匹配作为约束，实验证明这个算法比较有竞争力。
• 传统的建图方式是通过CAD(计算机辅助设计)，通过激光卷尺来测量，但是这种方式很慢并且测量人员会先入为主的认为建筑物都是直的，不能很好的描述空间的真实情况。使用SLAM可以迅速而准确地测量大小和复杂程度的建筑物，而手动测量则需要更长数量级的时间。
• SLAM在这一领域的应用并不是一个新的想法，也不是本文的重点。相反，本文的贡献是一种新的方法，以减少计算回环检测要求的激光距离数据。这种技术使我们能够绘制非常大的楼层，数万平方米，同时为运营商提供充分优化的实时结果。

其主要思想是将可能的子集表示为树中的节点，其中根节点表示所有可能的解，在我们的例子中是W。每个节点的子节点构成其父节点的一个分区，因此它们一起表示相同的可能性集。叶子节点是单例 （singletons）;每一个代表一个可行的解决方案。注意，该算法是精确的。它提供了与单纯方法相同的解决方案，只要内部节点c的得分©是其元素得分的上界。在这种情况下，无论什么时候一个节点是有界的，这个子树中都不存在一个比目前已知的最好的解更好的解。 —-Cartographer_Google的论文

• 对于一颗二叉树来说，形似如下图形式的顺序依次访问每个节点的搜索方式叫做DFS，深度优先搜索。

• 线性规划(Linear Programming 简记 LP)是了运筹学中数学规划的一个重要分支。自从 1947 年 G. B. Dantzig 提出 求解线性规划的单纯形法以来，线性规划在理论上趋向成熟，在实用中由于计算机能处理成千上万个约束条件和决策变量的线性规划问题之后，线性规划现代管理中经常采用的基本方法之一。 在解决实际问题时，需要把问题归结成一个线性规划数学模型，关键及难点在于选适当的决策变量建立恰当的模型，这直接影响到问题的求解。

• 线性规划问题的目标函数及约束条件均为线性函数；约束条件记为 s.t.(即 subject to)。目标函数可以是求最大值，也可以是求最小值，约束条件的不等号可以 是小于号也可以是大于号。

• 一般线性规划问题的数学标准型为

  m a x z = ∑ j = 1 n c j x j \begin{align} max \quad z = \sum^n_{j = 1} c_j x_j \tag{3} \end{align} maxz=j=1∑n​cj​xj​​(3)​

  s . t . { ∑ j = 1 n a i j x j = b i i = 1 , 2 , . . . , m x j ≥ 0 j = 1 , 2 , . . . , n \begin{align} s.t.\quad \begin{cases} \sum^n_{j = 1}a_{ij}x_{j} = b_i \quad i = 1,2,...,m \\ x_j \ge 0 \quad j=1,2,...,n \tag{4}\end{cases} \end{align} s.t.{ ∑j=1n​aij​xj​=bi​i=1,2,...,mxj​≥0j=1,2,...,n​​(4)​

• 例题

  某机床厂生产甲、乙两种机床，每台销售后的利润分别为4000元与3000元。生产甲机床需用A、B机器加工，加工时间分别为每台2小时和1小时；生产乙机床需用A、B、C三种机器加工，加工时间为每台各一小时。若每天可用于加工的机器时数分别为A机器10小时、B机器8小时和C机器7小时，问该厂应生产甲、乙机床各几台，才能使总利润最大？

  机床	利润	加工机器&工时(\h)
  甲	4000元	A (2)、B (1)
  乙	3000元	[A (1)、B (1)、C (1)]

  上述问题的数学模型：

  设该厂生产 x 1 x_1 x1​$ 台甲机床和 x 2 x_2 x2​乙机床时总利润最大，则 x 1 x_1 x1​,$ x 2 x_2 x2​应满足

  ( 目标函数 ) m a x z = 4 x 1 + 3 x 2 \begin{align}(目标函数) max \quad z = 4x_1 +3x_2 \tag{1}\end{align} (目标函数)maxz=4x1​+3x2​​(1)​

  s . t . ( 约束条件 ) { 2 x 1 + x 2 ≤ 10 x 1 + x 2 ≤ 8 x 2 ≤ 7 x 1 , x 2 ≥ 0 \begin{align}s.t.(约束条件) \begin{cases} 2x_1 + x_2 \le 10 \\ x_1 + x_2 \le 8 \\ x_2 \le 7 \\ x_1,x_2 \ge 0 \end{cases} \tag{2}\end{align} s.t.(约束条件)⎩ ⎨ ⎧​2x1​+x2​≤10x1​+x2​≤8x2​≤7x1​,x2​≥0​​(2)​

• 上述例题中变量 x 1 , x 2 x_1,x_2 x1​,x2​称之为决策变量，(1)式被称为问题的目标函数，(2)中的几个不等式是问题的约束条件，记为s.t.(即subject to)。由于上面的目标函数及约束条件为线性函数，故被称为线性规划问题。

• 可行解：满足4个约束条件的解，称为可行解。 而使目标函数达到最大值的可行解叫最优解。
• 可行域：所有可行解构成的集合称为问题的可行域，记为R 。

• 图解法（高中常用方法）

  简单直观，适用于二维决策变量，它有助于了解线性规划问题求解的基本原理。。对于每一固定的值z，使目标函数值等于z的点构成的直线称为目标函数等位线，当z变动时，我们得到一族平行直线。

  图解法简单直观,对于每一固定的值z，使目标函数值等于z的点构成的直线称为目标函数等位线，当z变动时，我们得到一族平行直线。等位线向上平移即可找到事目标函数有最大值的点。不难看出最优解为(2,6)，带入目标函数即可。

• 单纯形法（计算机常用方法）

  当变量变多时，我们不能简单的画出图像，并且为了能够让计算机计算，我们需要借助单纯形法。单纯形法是求解线性规划问题最常用、最有效的算法之一。

  线性规划之单纯形法描述

  这个算法的思想可以用几何术语描述如下：

  先在可行域中找到一个极点，然后检查一下是不是在邻接极点除可以让目标函数取值更佳。如果不是，当前顶点就是最优点，然后算法停止；如果是，转而处理那个能让目标函数取值更佳的邻接顶点。有限步以后，该算法要么发现了一个取到最优解的极点，要么证明了最优解不存在。

  参考《算法设计与分析基础》潘彦译 P267

• 如不加特殊说明，一般指整数线性规划。对于整数线性规划模型大致可分为两类：

  • 变量全限制为整数时，称纯（完全）整数规划。
  • 变量部分限制为整数的，称混合整数规划。

• 原线性规划有优解，当自变量限制为整数后，其整数规划解出现下述情况：

  • 原线性规划优解全是整数，则整数规划优解与线性规划优解一致。
  • 整数规划无可行解。
  • 有可行解（当然就存在优解），但优解值变差。

• 整数规划优解不能按照实数优解简单取整而获得。

• 例题1

  原线性规划为：

  m i n z = x 1 + x 2 2 x 1 + 4 x 2 = 5 , x 1 ≥ 0 , x 2