译文《Efficient Trajectory Optimization using a Sparse Model》

Efficient Trajectory Optimization using a Sparse Model

Christoph Rösmann1, Wendelin Feiten2, Thomas Wösch2, Frank Hoffmann1and Torsten Bertram1

摘要-“TEB”方法通过后续修改由全局规划器生成的初始轨迹来优化机器人轨迹。在轨迹优化中考虑的目标包括但不限于总路径长度、轨迹执行时间、与障碍物的分离、通过中间路径点以及遵守机器人动力学、运动学和几何约束。”TEB“明确地考虑运动的时空方面的动态约束，如有限的机器人速度和加速度。轨迹规划是实时运行的，“TEB”能够应对动态障碍和运动约束。“时间弹性带问题”被表述为一个尺度化多目标优化问题。大多数目标都是局部的，只与参数的一小部分相关，因为它们只依赖于机器人的几个连续状态。这种局部结构产生了一个稀疏的系统矩阵，允许利用快速有效的优化技术，如开源框架“g2o”来解决“TEB”问题。“g2o”稀疏系统解算器已成功地应用于VSLAM问题。这一贡献描述了g20框架在“TEB”轨迹修改中的应用和适应性。仿真和实验结果表明，该方法具有较好的鲁棒性和计算效率。

1 介绍

• 轨迹规划是根据机器人的运动学和动态运动约束找到最优的无碰撞轨迹。本文在全局规划器预先生成一个初始可行路径[1]的前提下，研究了路径修正问题。特别是在服务机器人环境中，对预先规划的路径进行动态修改比“离线路径规划”更受青睐。在线修正通过结合最新的传感器数据来应对动态环境的变化，从而对轨迹进行局部细化。在大多数实际应用中，由于局部、不完整的映射和动态障碍，环境模型会不断变化。此外，在实时应用中，大规模全局路径的(重新)计算往往是不可行的。这一观察结果导致了局部修改路径的方法，例如[2]，[3]提出的“elastic band”。
• 后来，该方法被推广到非完整运动学[4]，[5]，[6]和多自由度机器人系统[7]。[8]提出了一种利用优化技术对初始路径进行变形的方法。轨迹，即沿路径的速度，没有经过优化。时间参数用于控制优化过程中路径的修改。规划器考虑非完整约束。
• [9]通过明确考虑时间信息来处理轨迹的变形而不是路径。将变形分解为斥力避障步骤和连通性维护步骤。
  在此基础上，[10]提出了一种将外部变形与内部连通性力相结合的单步方法。这两种方法都支持一般的状态转换模型，并允许时空障碍规避。相比之下，我们的方法是基于一个图优化公式，与一般的优化求解器和时间最优是一个明确的目标。
  其他直接优化轨迹的方法见[11]，[12]。在我们的例子下，参数路径增加了速度的分布，符合平台的运动学约束。该方法从全局规划器找到的初始路径开始，并通过[13]中使用的紧凑样条路径模型表示它。该路径模型为优化提供了一组更高层次的参数，迭代地适应曲率连续路径的形状，以减少运行时间等目标函数。
  我们工作的主要区别在于，它将解析模型的精度转化为离散化轨迹模型，从而可以采用高度复杂、高效的优化算法，实现实时路径优化。
• 最近处理多自由度机器人手臂的轨迹修正方法使用了位形空间中轨迹的离散表示(见[15]，[16])。所提出的目标函数包含一个有限差分矩阵，以平滑所得到的轨迹，并额外满足约束。如避障。CHOMP算法依赖于协变梯度下降方法，明确要求每个目标的梯度，而STOMP使用随机轨迹优化技术，而不需要明确的梯度知识。这两种方法通过定义特定的离散化和任务持续时间，仅以隐式方式包含时间信息。我们的方法的区别在第四节中详细介绍。
• 在[17]中，作者引入了一种名为“TEB”的新方法，该方法明确地用时间信息增加了“EB”。提出的扩展允许考虑机器人的动力学约束和直接修改轨迹而不是路径。“TEB”被定义为一个尺度化的多目标优化。底层的结构优化问题是稀疏的，因为大多数目标都是本地,他们只取决于几个连续位姿而不是整个轨迹。
• 数值数学为具有稀疏结构的优化问题提供了有效的算法，这些算法已成功应用于“视觉同步定位和映射”(vSLAM)或“稀疏束调整”(SBA)[18]等任务。[19]引入了一个开源的c++框架称为“通用(超)图优化”(g2o)，它解决了基于图的非线性优化问题。使用多目标优化框架的一个明显优势是目标和约束的模块化表达。
• 针对基于超图的非线性优化问题，本文提出了“TEB”方法，并在差动驱动的移动机器人上实现了g2o。该机器人在具有三个全局自由度和两个局部自由度的平面环境中运动。一般来说，TEB适用于高维状态空间。通过考虑时间信息，TEB明确地考虑和控制机器人的速度和加速度。
• 我们首先详细介绍了[17]中描述的TEB的一般概念，特别是将问题映射为超图表示，确定TEB的初始条件和算法实现。第三节介绍了“TEB”与g20框架之间的联系。第四节给出并分析了实验结果。虽然本文呈现的实验描述了一个非完整的机器人，但该方法并不局限于任何特定的机器人运动学或动力学结构。最后，第五部分总结了TEB的成果，并对进一步的工作进行了展望。

2 TEB

A. TEB的定义

图一：（a）位姿序列与时间差 （b）考虑路径点和障碍物的大场景

经典的“EB”是用n个中间机器人姿态si= [xi，yi，βi]T∈R2×S1的序列来描述的，下面表示为机器人在全局坐标系({map})中的位置xi,yi和方向β i所定义的位型，图1(a)):

TEB方法强化了这种表示方法，具体方式是“合并两个连续位姿之间的时间间隔，从而生成一个由n-1个时间间隔ΔTi组成的间隔序列：