Cartographer是一个提供2D和3D实时定位和制图的系统，它可以跨多个平台和传感器进行配置。同时它是一种基于非线性的算法优化。与其他算法（例如gmapping（粒子过滤器））相比，它可以提供更高的映射的准确性。Cartographer将Branch-and-Bound引入了Lidar-SLAM系统，从而减小了根据激光点数据计算闭合回路的计算量。由此Cartographer可以在2D的Lidar-SLAM中提供实时环路闭合，可以在长时间场景中显著提高定位的精度。
  针对该比赛，我们使用了Cartographer的localization-only模式，该模式会针对现有的本地地图进行修改，而不会构建新的地图。通过这种模式，我们可以利用构建好的地图进行定位，从而提高定位的准确性，减少建图和定位所需的计算量。
  因为赛场上可以同时拥有两辆我方机器人，因此我们还使用了multi-trajectories SLAM。通过这种方式，Cartographer可以从两辆车上同时获取SLAM信息，通过两辆车的路径和SLAM信息来达到更高的定位精度。
  我们在实物机器人上进行测试，最终采用Cartographer实现建图的效果如下图（播放速度：5X）：

  在路径规划中，需要依靠地图来计算出一条路径，但是依靠一张静态的环境全局地图是不够的，因此我们需要增加不同的地图层来完成路径规划。
  我们在原先静态地图层（Static Map Layer）、障碍物地图层（Obstacle Map Layer）、膨胀层（Inflation Layer ）的基础上增加了加成区层（Buff Layer）。同时实现在全局路径规划使用的代价地图上应用加成区层，在全局路径规划时完成躲避惩罚区和敌方的加成区域。

  传统的A*算法的关键是计算每个节点的优先级，主要通过公式f(n) = g(n) + h(n)进行计算，其中f(n)是节点n的综合优先级，g(n)是节点n距离起点的代价，h(n)是节点n距离终点的预计代价。
本项目采取的数据结构为priority_queue，用于存储扩展过程中的节点。

  基于现有的A算法，我们在计算g_score_.at(current_index)时将每个格中可能出现障碍物的概率作为附加更新项，这种做法可以让机器人尽可能避开障碍物出现的位置。
  A 算法优点在于对环境反应迅速，搜索路径直接，是一种直接的搜索算法。更重要的是它永远可以收敛到最优点，下面我们作以简单的证明。
  假设次优点G2在open表里面生成的结点，n是一个结点（它是离最优点G有最近距离的结点），则有：

  因此如果有最优点，那么A*算法总是会最先找到最优点。

  多机器人高速运动的过程中，需要考虑协同路径规划冲突以解决多条路径冲突，提高机器人路径规划的效率和运动过程中的安全性。
比赛场地上除了有已知的静态障碍物外，移动过程中的机器人也会成为其他机器人路径上的动态障碍物。因此，在规划机器人路径时需要将潜在的冲突点视作不可行的障碍物。

  结合实际场景分析，路径可能冲突的情况为：首先我们需要判别两辆小车规划出的路径是否有可能冲突。假设我们知道两个车的位置和当前全局规划的路径，那么对于任意一个机器人，只要能够保证在任一时刻他们不会到达相交的点。注意这里的相交点指的是时间和空间上的相交，并非单纯的空间上的相交。同时为了让机器人尽可能的分散，我们需要让机器人规划的路径尽可能的分散开来以减少相撞和冲突的可能。因此我们将冲突检测分为两个等级：

• : 防止时间和空间上的相交；
• : 尽可能的远离另一个机器人。

  为了更好的与进行耦合，我们需要为这两个检测增加相同单位的量化标准，因此对于，我们只需要像传统那样使用语句跳过该点（即视将该点视为绝对障碍物）,对于我们可以为协同机器人的路径增加额外的。
  首先考虑，通过的介绍我们可以得出，每个路径上的点均包含两个，他们分别是和。因为我们只需要得知机器人到达该点时的，所以对于每个点我们只需要考虑他们（该值可以理解为机器人到达该点需要的时间）的值。 对此当我们进行路径搜索时，当检测到某个点已经被占有时我们只要让他们在时间上不会相交他们就会以一个先后的顺序通过该点，这样就可以避免碰撞。对此我们可以得到约束：

      （1）

 

  这里的和分别为两个机器人从起点到达冲突点的代价，为机器人离开冲突点的代价。

  同时因为机器人是有一定的体积的，因此我们需要让上述约束建立在机器人的体积上。我们假设进行规划的机器人所搜索的路径点的位置为，冲突点的位置为，机器人的最大直径为，那么我们执行上述约束（1）的条件应为：

    （2）

  接着我们考虑，因为机器人的移动都是由代价来进行量化处理的，因此我们只需要在机器人之间的欧几里何距离小于机器人的最大直径时为其增加额外的即可。因此的约束为：

（3）

  这里进行规划的机器人的所搜索的路径点的位置为，协同机器人所规划出的路径上的点的位置为。对于我们可以通过函数表述为：

    （4）

  这里的为系数，为某一常数，通过调节与的值可以改变的大小和辐射的幅度。

  通过算法进行实现，我们可以得到与传统的路径对比图如下（图（13）为传统A*算法下多机器人进行路径规划得到的路径，图（14）为基于冲突检测的多机器人协同规划所得到的路径）：

  可以看出在起点和终点相同的情况下，通过冲突检测规划出的路径明显绕开了协同机器人所规划的路径。

  机器人在进行对战的过程中很可能会因为不可控的因素被撞到一些无法预料的位置。这些位置很可能会让正常的路径规划无法搜索出任何路径，因为机器人的中心处于cstmap的绝对障碍物这一层（也就是说它认为自己是被障碍物包围的）。对于这种情况机器人整局比赛都无法再次进行正常的比赛，这无疑是致命的。
  为此我们在此对算法进行改进，我们通过来记录连续规划失败的次数，使用半径来决定路径规划中的需要修复的范围。因此当大于某一常数时我们将会对路径规划进行修复。这一修复行为具体为清除半径内所有障碍物，并且用一个相对较大的来替代，这样一来就可以在不违背路径规划最优的原则下，搜索出一条走出“障碍物”内部的路径。

  通过全局规划器我们得到了起始点以及目标点状态，现在我们可以采用TEB算法（Timed-Elastic-Band）在其中插入多个控制机器人姿态的控制点，并且在点与点之间定义运动时间Time来显示轨迹的运动学信息。
TEB基于姿态搜索最优方案，基本思想是在车辆周围的空间内散布大量的姿态，再通过搜索树搜索最优方案。
通过基于TEB算法的局部规划器，小车找到的路径是时间最短，距离最短的，并且可以满足远离障碍物，同时限制速度与加速度使轨迹满足小车底盘的运动学约束的要求，在比赛中取得更好的表现。

  局部规划器中主要采用了TEB算法，完整的流程如下。

图（15）TEB算法流程图

  假设机器人在世界坐标系的位置和姿态分别为和 ,如下图所示,因此我们可以定义机器人的位姿configuration为

  由此，可以定义出空间内一系列的configuration序列为,定义相邻序列的时间间隔为，相应的时间序列即为。

之后可以整合位姿序列和时间序列为,通过加权优化我们可以得到最优的路径点,即:

其中，为最优的结果，为考虑各种约束的目标函数,为目标函数各项的权值.

• 跟随路径和避障约束

  在路径规划中存在两个约束用来跟随已知的全局规划路径和避障。跟随路径施力将elastic bands拉向全局路径，避障约束施力使得elastic bands远离障碍物。这两种约束将以乘法函数来实现：

• 速度和加速度约束

  速度和加速度组成的动力学约束也可以类似的使用运动学约束的乘法函数表示，机器人运动的平均线速度和角速度可以用下列公式计算：

• Non-holonomic运动学约束

  本项目使用的机器人是沿着若干弧段组成的平滑的轨迹运动，小车相应的目标函数表达式如下：

• 最快路径约束

  目标函数即为最小化时间间隔序列的二次方，它使得机器人可以获得最快的路径，路径上的各configuration点在时间商均匀分开，而非传统的空间上求最短路径。

  结合上面的流程图，TEB算法通过增加约束条件完成初始化阶段,之后在每次迭代过程中动态添加或删除机器人的位姿configuration将关于空间和时间的分辨路调整至余下的轨迹长度和规划的范围内，并添加hysteresis实现避障。至此，优化问题就被转化为了一个hyper-graph问题，可以通过g2o中关于大规模稀疏矩阵的优化算法解决。
在验证路径的正确后，就可以将计算控制量和直接用于机器人系统。在每次迭代开始的时候，接受新的路径节点，机器人便可以更新并确定新的路径。

---