【论文翻译】Efficient Trajectory Optimization using a Sparse Model——使用稀疏模型对有效轨迹进行优化(TEB局部规划)

          摘  要  : “TEB”方法针对全局路径规划器生成的初始轨迹进行后续修正，从而优化了机器人的运动轨迹。在轨迹优化问题中考虑的目标包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动态、运动学和几何约束的符合性。“TEB方法”明确考虑了运动状态下时空方面的动态约束，如机器人的速度和加速度是有限制的。路径规划是实时进行的，“TEB”能够解决动态障碍和运动约束的问题。将“TEB”表述为一个多目标优化问题。大多数目标都是局部的，只与一小部分参数相关，因为它们只依赖于几个连续的机器人状态。这种局部结构产生了一个稀疏的系统矩阵，可以使用快速高效的优化技术，例如开源框架“g2o”来解决“TEB”问题。“g2o”作为一个稀疏系统求解器已成功应用于VSLAM。这篇文章描述了，在“TEB”修正路径问题中，g2o框架应该如何进行修改和应用。

• 1 . Introduction

  • 路径规划求解一个最优的无碰撞轨迹，该轨迹应该符合机器人的运动学和动态运动约束。本文着重讨论了轨迹修正问题，假设全局路径规划器已经提前生成了初始化的可行路径。特别是在服务类机器人的问题上，“针对提前规划好的路径进行动态修改”比“离线路径规划”更得到青睐。结合实时的传感器数据来处理动态环境变化，在线修改轨迹，从而进行局部优化。在实际应用中，由于局部性问题、地图的不完整性和障碍的动态特性，环境模型会持续变化。此外，大规模全局路径的重新计算在实际应用中通常是不可行的。这种状况催生了一些局部路径规划的方法，如“elastic band”方法。
  • 随后，原来的方法被扩展到非完整运动学和具有多个自由度的机器人系统。《Reactive path deformation for nonholonomic mobile robots》一文提出了一种利用优化技术使初始化路径变形的方法，此法中路径上的速度没有被优化。时间参数用于控制优化过程中路径的修改。规划器考虑非完整约束。
  • [9]通过明确考虑时间信息来处理路径的变形，相比于单纯考虑路径。使用排斥力和连接性保持步骤，将轨迹优化分解为避障步骤。在此基础上，[10]提出了一种将外部变形与内部连接力相结合的单步方法。两种方法支持通用的状态转换模型，同时支持时空避障。不同的是，我们的方法基于图优化公式，与通用的优化解算器一起执行，并且时间最优性是一个明确的目标。其他的直接轨迹优化方法见[11]，[12]。在我们的例子中，参数当中增加了速度的分布，这与平台的动力约束有关。我们的方法从一个全局规划器生成的初始路径开始，然后用一个紧凑，曾经出现在[13]中的“spline-based path model ”模型表示它。该轨迹模型向优化器提供一组高级参数，以迭代的方式适应弯曲路径的形态，从而减少目标函数比如运动时间。我们工作的主要不同之处在于：它在离散轨迹模型的模型解析精度上有所改进，这使得它可以采用一种高度复杂、高效的优化算法，实现实时的路径优化。
  • 近期的轨迹修正方法（涉及到的机器人手臂有很多自由度）对配置空间中的轨迹进行了离散化的表示。所提出的目标函数包含了一个有限差分矩阵，目的是对所得到的轨迹做平滑处理，同时满足额外的约束，比如避障。CHOMP算法采用协变梯度下降法，对每个目标的梯度有着明确要求，而STOMP采用的是随机轨迹优化技术，对梯度没有明确要求。这两种方法都只以隐式的方式包含了时间信息，具体做法是定义一个特定的离散化和任务持续时间。我们所采用方法的不同之处在第四节中详细介绍。
  • 在[17]中，作者介绍了一种新的方法，称为“timed elastic band”，它用时间信息强化了原本的“elastic band”方法。这种新的扩展把机器人的动态约束纳入考虑，同时直接修改的是轨迹而不是路径。“TEB”是一种多目标的优化方法。底层优化问题的结构是稀疏的，因为大多数目标都是局部的，它们只依赖于几个连续的配置，而不是整个轨迹。
  • Numerical mathematic为稀疏结构的优化问题提供了有效的算法，这些优化算法已成功地应用于诸如vSLAM或“sparse bundle ad- justment”(SBA)等问题。[19]引入了一个开源的c++框架，称为“通用图优化”(g2o)，它解决了基于图的非线性优化问题。使用多目标优化框架的一个明显优势是目标和约束的模块化设计。
  • 本文根据"基于超图的非线性优化问题"提出了“TEB”方法,同时在差动驱动的移动机器人上利用g2o框架对算法进行了实现。机器人在一个平面环境中移动，有三个全局自由度和两个局部自由度。一般来说，TEB适用于高维状态空间。通过把时间信息纳入计算，TEB明确地考虑和控制了机器人的速度和加速度。
  • 我们首先详细地介绍了TEB算法的一般概念，着重于将问题映射到一个“超图”的表示，同时确定了TEB算法的初始化条件和算法的实现。第三节介绍了“TEB”与g2o框架之间的联系。第四节给出并分析了实验结果。虽然在本实验中的使用的是非完整的机器人，但该方法不局限于任何特定的机器人运动或动态结构。最后，第五部分总结了TEB的成果，并对以后的工作进行了展望。

• 2 .TIMED ELASTIC BAND

  • A. Timed Elastic Band 的定义(TEB)

     

    • 
    • 经典的“elastic band”用由个n的机器人位姿组成的序列，其中xi，yi是机器人位置，βi被定义为全局中机器人的方向（图1a）。序列记为
    •  TEB方法 强化了这种表示方法，具体方式是“合并两个连续位姿之间的时间间隔，从而生成一个由n-1个时间间隔ΔTi组成的间隔序列：”。
    • 每个时间间隔表示机器人需要从当前位姿转换到序列Q中的下一个位姿所需要的时间(图1(a))。TEB被定义为两个序列的元组:
    • 关键思想是利用加权和模型对TEB进行调整和优化，具体方式是“用一种标度的多目标优化方法，在配置和时差方面进行了改进”，其中B∗表示被优化的TEB序列结果，f(B)表示全局目标函数。
    • [17]中给出了适合于TEB的成分目标函数 fk ，有如下两种基本类型：用惩罚函数表示的速度和加速度限制&与轨迹有关的目标函数比如最短的路径，最快的执行时间或间隙的障碍。在机器人框架(如ROS)中，稀疏约束优化算法并不能实现自由的使用。这推动了g2o框架的采用，在这个框架中：约束被制定，以分段连续可微成本函数为目标，并惩罚违反约束的行为。
  • B. Problem representation as a Hyper-Graph(将问题表示为超图)
    • 根据式(4)、(5)，TEB被定义为一个标量化的多目标优化问题。大多数需要的目标函数依赖的参数取决于邻近位姿的子集。
      • 速度(加速度)约束取决于两个(三个)连续的位姿和一个(两个)时间差。
      • 障碍物的清除和在中间路线上进行导航影响单个位姿和它的k个最近的邻居(实际上大约3-5)。
      • 遵守机器人的非完整约束涉及到了两个相邻的位姿，这两个位姿需要位于一个常曲率的普通圆弧上。
    • 最快和最短路径是局部结构的例外，因为这些目标需要全局依赖于所有的参数。具有均匀时间间隔的最短路径结构，是通过最小化 所有时间差之和的平方 或者  所有时间差平方之和得到的。
    • TEB的局部性产生了一个稀疏的系统矩阵，该矩阵用超图表示，其中的节点对应于位姿和时间间隔。包含了同一个目标函数需要的参数的节点通过对应的多边进行连接。下面，将式(4)转化为超图。超图的定义意味着，由一条边连接的节点数不局限于普通图形的两个节点。超边是传统的边的拓展，因为它连接了多个节点。每个目标函数依赖于TEB状态序列的子集(位姿和时间差)，超边表示目标函数fk，并连接多个节点，这些节点对应于位姿和时间差异。                                     
    • 图2(a)显示了一个超图的简单例子，超图表示约束和一个与两个位姿s0，s1相关的目标，时差ΔT0和一个障碍O1。注意，我们的特定实现方法将每个位姿节点划分为位置节点和方向节点，便于目标函数模块化的激活和取消激活。速度目标函数把距离上界设置在s0和s1之间，机器人在间隔ΔT0内可以到达。速度-多边(fvel)和加速度-多边(facc)用于描述机器人的动态方面。“障碍物与其最近位姿之间的距离(o1和s1之间)”是由一个最小的间隔距离来限定的，这样可以保证一个不发生碰撞的路径。障碍物的位置是根据机器人感知层提供的传感器数据推断出来的。对应的障碍物不受图形优化的影响，在图2(a)的双圈上表示。更大的TEB超图如图2(b)所示。依照每个目标函数的权重，把每个多边中的目标函数加权表示为整体目标函数。除了障碍节点是固定的以外，路径点配置p和初始状态s0也是固定的。在我们的应用程序中，我们在控制循环中使用规划器，因此初始状态由机器人的当前配置给出。TEB实时调整原始机器人的轨迹(t = 0)并且在时间间隔t∈(6、12)通过改变原始轨迹远离障碍来避免与人发生碰撞。

  • C. Control flow（控制流）

    • 图3为实现的TEB的控制流。  

    • 在初始化阶段，通过添加动态和运动学约束的默认定时信息，将初始路径强化为初始轨迹。在我们的例子中，初始轨迹是由旋转后再进行平移的分段线性段组成的。这种以多边形的形式表示的路径通常由probabilistic roadmap planners 提供。

    • 在每一个轨迹修正步骤中，算法会动态地添加新的配置或移除配置，为了将空间和时间的分辨率调整到剩余的轨迹长度或规划范围。机器人对障碍和路径点的感知与TEB状态有关。注意，通过“通过寻找TEB和障碍物预测之间的最小时空距离，而不是使用实际的位姿测量”的方法结合障碍运动模型（例如恒定速度）通常会得到更直观的解决方案。为了测试我们的规划器，并分析它如何实时地对障碍物的进行感知，我们在第四部分的实验中将障碍视为静态的。我们对动态障碍的方法的扩展很简单。将优化问题转换为超图，并使用下一节详述的g2o框架来解决。g2o框架在批处理模式下优化了TEB，因此每次迭代都会生成一个新的图，并使用最新的解决方案进行初始化。我们建议在一个机器人控制周期内对轨迹修改步骤进行多次迭代（在我们的例子中，四个循环包括5次Levenberg-Marquardt迭代）。

    • 对优化后的TEB进行验证，检查是否违反了硬约束，如果违反，在这种情况下机器人要么停止，要么重新调用运动规划器。成功验证后，立刻根据TEB的下一个位姿对控制变量v和ω进行计算，并且作为运动指令发送给机器人。在每次修改前，重新初始化阶段检查新的或修改的路径点；如果路点不是来自静态地图，而是从以机器人为中心的角度，用车载相机或激光雷达来感知地标，那么它是有用的。

• 3 .  g2o图优化

  • 为了解决具有以下特殊结构的非线性优化问题，我们开发了g2o:
  • 
  • x表示要优化的参数，zij 表示两个参数块xi和xj之间的约束，而Ωk表示约束的信息矩阵。向量ek（xi，xj，zij）提供了约束和参数之间的误差。注意，(6)是非线性最小二乘优化中常用的目标函数。
  • 使用g2o框架进行teb优化所需要的准备工作，见(4)，根据(6)。x被TEB-tuple B所取代。对于标量误差项，Fk 简化成 Fk = Ωk *ek^2,其中Ωk = γk，ek = fk^1/2(使用（4）式)。注意，对于轨迹优化，（6）中的参数xi是由TEB-State（si，ti）在（3）式中给出的。
  • g2o框架需要定义节点和边。表1提供了TEB节点的概述。bi表示位置向量[xi , yi ]T。对于每个节点，都会定义一个增量，将变量的局部参数化映射到其初始值。在角度递增的问题上，在将旋转增量添加到之前的方向角后，再把角度规范化到间隔[−π,π)。其余的变量用欧几里得坐标表示，因此简单的加法就足够了。
  • g2o框架要求对每条多边的配置进行误差函数ek = √fk的定义和权重Ωk = γk 的定义。在本文的实验中，利用g2o框架的数值近似，计算了(6)中误差函数ek的雅可比矩阵。在今后的工作中，可以提供解析形式的导数来提高优化效率。
  • 等式(7)用levenberg - marquardt方法[19]进行求解:
  • 表示系统矩阵(Hessian)。λ是一个由g2o-framework自动选择的阻尼因子。x∗代表最优TEB-states和误差项。Jk表示在当前解的线性化过程中得到的雅可比矩阵。TEB的一个重要性质是H的稀疏性。
  • 

  • 图4(a)给出了TEB系统矩阵H的一个例子。它是稀疏的，只有15%的非零元素。在本例中，前141个状态对应于47个配置xi，而最终状态142-189表示时间差ΔTi。这些最终状态与最快轨迹的目标有关，因此这个块是密集的，连接与TEB的维数关联，成二次增长。

  • 利用sparse Cholesky decomposition algorithms和先验排序方法，对式(8)进行了数值高效求解（如图4b）。g2o框架提供了两种基于Cholesky decomposition 的求解器:CHOLMOD和CSparse。在第一个实验中，两个求解器在优化的运行时行为方面没有显著差异。由于CSparse求解器速度稍快，所以本文的实验都是以CSparse为基础的。目前还不清楚哪种解算器更适合大维度空间( [19]更喜欢小维度适用于CSparse，更高维度适用于CHOLMOD)。

•  4 . 实验和结果

  • 仿真和实际实验的重点采用的是一个带有差动驱动的非完整机器人。机器人模拟器是一台运行英特尔酷睿i7 2x2.3GHz和4GB RAM的虚拟机。真正的机器人实验用的是Pioneer 2 with a Siemens Lifebook s6410, Core2Duo, 2.4GHz, 2GB RAM。该机器人配备了一个Hokuyo激光雷达。
  • 对于优化问题中所建议的目标的权重的选择，解决方案是鲁棒的。我们将非完整约束设置为1000，所有其他权重设置为1。
  • A . 仿真实验
    • 
    • 在机器人导航中，避障是一项重要的任务。为了实现动态避障，在合适的场景中对TEB进行实时分析。图5显示了两张TEB轨迹修正的快照，而从机器人的角度来看，一个障碍物使原来的轨迹向左变形
    • 在图5场景中，单次轨迹优化循环的平均运行时间是2.1 ms ± 0.4 ms。在1000个循环的整个模拟过程中，障碍在TEB上来回移动。计算时间不变，并没有受到动态障碍的影响。
    • 
    • 图5中的(绿色)向量表示差分驱动机器人两个轮子的平移速度。每个车轮的速度和加速度曲线如图6所示。轨迹满足最大速度限制在1.4m且最大加速度限制在0.3 m/s^2的约束条件。
    •  
    • 另一个关于性能的方面是：每次迭代的平均运行时间依赖于TEB的维数，TEB的维数随配置的数量线性增长。结果如图7(a)所示。对于10000多个状态，对应大约2500种配置。在路径长度约5m时，运行时间超过机器人控制周期约20-30ms。然而，在实际应用中，轨迹的时空分辨率要低得多，因为对初始轨迹的改进只有在向前几米的距离内才有意义（这相当于机器人的感知范围）。
    • 图1(b)显示了一个由两个静态障碍物和四个中间路径点组成的更大的场景。轨迹满足所有规定的约束条件。
    • 图8展示了与g2o框架相结合的稀疏模型的性能和效率。
    • 图8(a)所示，12条初始轨迹被实时地优化，以选择最佳的候选轨迹(例如参考优化成本)。轨迹修正循环（见图3）在虚拟系统的不同线程中被执行。为了降低成本，初始轨迹优化到接近最优轨迹(图8(b))，前两轮迭代需要218ms(见图7(b))。对于每个随机移动障碍的后续迭代只需要48ms± 4 ms。注意，这条轨迹比之前的图5中要长得多。
    • 作为基准，将TEB的计算量与相同场景下的STOMP([16])进行比较(图8)。STOMP最初是为机器人手臂制定的，因此要优化的关节变量被平面上的x和y所替代。注意，STOMP不依赖于梯度信息且不需要可微目标函数。相比之下，g2o框架近似于数值梯度，需要可微的目标函数。我们的STOMP使用了20个随机的轨迹来执行更新步骤。
    • 该实现包含了与障碍成本函数相结合的加速度矩阵。起初，STOMP的目标是实现无碰撞轨迹，而不是快速轨迹。显然，这对于移动机器人的运动规划是不够的。有可能增加更多的状态和目标。然而，即使只考虑无碰撞路径的STOMP-2D，他也被TEB超越。
    • 图8(c)显示了使用STOMP针对12个初始轨迹的优化结果。每条轨迹都由80个2d点组成，这相当于每一个TEB的平均配置数量(TEB使用了动态调整)。固定的配置数与加速度矩阵的权重相结合，影响了任务的运行时间。对于不同的场景，权重必须进行调整(见图8(c)中较长的轨迹)。利用所提出的目标函数，我们的STOMP无法处理TEB能够胜任的不连续初始轨迹。运行时间与图7(b)中的TEB进行比较。TEB执行优化的速度明显快于STOMP。STOMP参数的直观选择使得它们在2D情况下与TEB有很大的可比性。此外，我们还实现了CHOMP的2d版本(没有Hamilton Monte Carlo)进行比较，但对于足够的空间分辨率，在上述场景中得到的结果并不健壮。

  • B .  机器人实验

    •  

    • 利用时间信息增强弹性带，使用g2o框架，实现机器人的实时轨迹调整和控制。图9显示了一个真实的机器人实验的一系列快照，在这个实验中，一个人在场景中行走(类似于前面的模拟场景，有一个障碍物)。

• 5 . 结论及进一步工作

  • 本文介绍了一种基于TEB的实时轨迹修正方法，重点介绍了g2o框架的实现。TEB的创新之处在于利用时间信息增强经典的弹性带。因此，不仅可以考虑路径的几何约束和运动学约束，还可以同时考虑移动机器人的动态约束。g2o为稀疏系统结构提供了算法和求解器。在本文中我们证明了TEB具有这样一个稀疏的系统结构，因此它可以通过g2o框架有效地解决。该算法实时运行，直接为底层机器人运动控制器生成命令。该方法具有高度的灵活性，易于适应不同机器人运动学和应用要求。
  • 未来的工作致力于进一步改进运行时间。第一种方法是为优化算法提供解析雅可比矩阵。二是将稀疏TEB系统矩阵的先验排序从一个优化周期重用到下一个优化周期。三是根据规划范围，在时间间隔长度和模型细节程度上动态调整TEB的分辨率。为了达到机器人运动控制的目的，只有接下来的几个状态是相关的，因此遥远的未来的远程配置是在更粗的范围内进行的。
  • 该方法的一个更基本的变化是使用了稀疏约束优化框架。这使得目前关于惩罚函数的约束公式过时了。