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组成的间隔序列：

每个时间间隔表示机器人在序列Q中从当前位姿过渡到下一个位姿所需的时间(图1(a))。TEB被定义为包含两个序列的元组:
其核心思想是采用加权和的多目标优化方法，从位姿和时差两个方面对TEB进行调整和优化:
其中，B∗表示优化的TEB, f(B)表示底层的全局目标函数。

[17]中给出了适合TEB的分量目标函数fk，有如下两种基本类型:以惩罚函数表示的速度和加速度限制等约束，以及关于轨迹的目标函数，如最短路径、最快执行时间或障碍物清除。在*使用的机器人框架(例如ROS)中，稀疏约束优化算法并不容易获得。这推动了采用g20框架，在该框架中，约束被表述为以分段连续、可微成本函数为目标的目标，以惩罚违反约束的行为。

B 将问题表示为超图
根据式(4)、(5)，将TEB定义为一个标量化的多目标优化问题。大多数需要的目标函数依赖的参数取决于邻近位姿的子集。

• 速度(加速度)约束依赖于两个(三个)连续的位姿和一个(两个)时间差
• 从障碍物上清除和在中间路线上进行导航影响单个位姿和它的k个最近的邻近位姿(实际约3-5)。
• 遵守机器人非完整约束涉及到两个相邻的位形，它们需要位于一个常曲率的公共圆弧上。

最快和最短路径是局部结构的例外，因为这些目标全局依赖于所有参数。通过最小化所有时间差的和 或 时间差的平方和来获得具有时间均匀间隔位姿的最快路径。
TEB的这种局部性导致了一个由超图表示的稀疏系统矩阵，其中节点对应于位姿和时间间隔。包含相同目标函数的参数的节点由相应的多边连接。将式(4)转化为一个超图。超图的定义意味着由一条边连接的节点数量不像传统图那样仅限于两个节点。超边是传统边的延伸，因为它彼此之间连接多个节点。每个目标函数依赖于TEB状态的一个子集(位姿和时差)，一个超边代表目标函数fk以及连接对应于其评估参数位姿和时差的节点。
图2：超图结构：节点（圆）和多边（矩形）

图2(a)是一个简单的超图示例，表示与两个连续位姿s0, s1有关的约束和目标，它们的时差ΔT0以及一个障碍物o1∈R2。请注意，我们的具体实现将每个位姿节点划分为位置节点和方向节点，以促进目标函数的模块化激活和停止激活。速度目标函数为机器人在分配的时间ΔT0内能够行走的s0和s1之间的距离设定了一个上界。速度-多边(fvel)和加速度-多边(facc)捕捉动态方面。障碍物与其最近的位姿（在例子o1和s1中）之间的距离从下面以保证无碰撞路径的最小间隔为界。障碍物的位置是由机器人架构中的感知层提供的传感器数据推断出来的。对应的障碍不受图优化的影响，如图2(a)中的双圆所示。图2(b)显示了对大多数实现的目标函数的TEB超图的较大提取。将每个多边中的目标函数按其权重表示为总体目标函数。除了固定的障碍节点外，路径点位姿p和初始状态s0也是固定的。在我们的应用中，我们在控制回路中使用规划器，因此初始状态由机器人当前的位姿给出。

C 控制流

图3为实现的TEB控制流程。在初始化阶段，初始路径被强化为初始轨迹，方法是根据动力学和运动学约束添加默认的定时信息。在我们的例子中，初始轨迹是由带有纯旋转和平移的分段线性分段组成的。这种以多边形表示的路径通常由概率路线图规划器[22]提供。

图3 TEB实时的控制流

在每个轨迹修改步骤中，算法动态地添加或删除新的位姿，以调整空间和时间分辨率到剩余的轨迹长度或规划范围。最近机器人对障碍物和路径点的感知与TEB状态有关。请注意，通过寻找TEB和障碍预测之间的最小时空距离，而不是使用实际的姿态测量，结合障碍运动模型（例如恒定速度）通常会得到更直观的解决方案。为了测试我们的规划器，并分析它如何实时地对障碍物的进行感知，我们在第四部分的实验中将障碍视为静态的。我们对动态障碍方法的扩展很简单。优化问题被转化为一个超图，并在接下来的章节中使用更详细描述的g20框架来解决。g20 框架以批处理方式优化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-元组 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。对于每个节点，都会定义一个增量，将变量的局部参数化映射到其初始值。在角度递增的问题上，在将旋转增量添加到之前的方向角后，再把角度规范化到间隔[−π,π)。其余的变量用欧几里得坐标表示，因此简单的加法就足够了。
表1 TEB节点综述
g2o框架要求对每条多边的位姿进行误差函数ek = √fk的定义和权重Ωk = γk 的定义。在本文的实验中，利用g2o框架的数值近似，计算了(6)中误差函数ek的雅可比矩阵。在今后的工作中，可以提供解析形式的导数来提高优化效率。
等式(7)用levenberg - marquardt方法[19]进行求解:H表示系统矩阵(Hessian)。λ是一个由g2o框架自动选择的阻尼因子。x∗代表最优TEB-状态和误差项。Jk表示在当前解的线性化过程中得到的雅可比矩阵。TEB的一个重要性质是H的稀疏性
图4(a)给出了TEB系统矩阵h的一个例子，它是只有15%的非零元素稀疏的。

在这个例子中，前141种状态对应于47种位姿xi，而最终状态142-189表示时差ΔTi。这些最终状态与最快轨道目标有关，因此该块是密集的，连接随着TEB的维数二次增长。
方程(8)通过稀疏Cholesky分解算法和先验排序(如AMD，图4(b))[20]，[21]以数值有效的方式求解。g2框架基于Cholesky分解提供了两种不同的求解器: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/s，且最大加速度限制在0.3 m/s^2的约束条件。

图7：（a）随着TEB维数增加，每次TEB循环的运行时间（b）12个轨迹的总成本随着时间流逝(缩放到[0…1])

另一个重要的性能方面是，每次迭代的平均运行时依赖于TEB的维度，TEB随着位姿的数量呈线性增长。通过在相同的场景中增加位姿的数量(更高的密度)来分析这种关系。结果如图7(a)所示。为10000多个状态，对应约2500种结构，在一个路径长度约5m，运行时间超过机器人控制周期约20-30ms。然而，在现实应用中，轨迹的时空分辨率明显较低，因为初始轨迹的细化只有在机器人感知范围内的几米内才有意义。
如图1(b)所示，一个更大的场景由两个静态障碍和四个中间路径点组成。轨迹满足所有的约束条件。

图8 ：12种不同轨迹的优化

图8展示了本文提出的稀疏模型结合g20框架的能力和效率。
如图8(a)所示的12条初始轨迹被实时优化，以选择最佳的候选轨迹(例如，考虑优化成本)。轨迹修改周期(如图3所示)在仿真系统的不同线程中执行。为了将初始轨迹的成本降低到接近最优轨迹(图8(b))， TEB的前两轮迭代需要218ms(见图7(b))。对于每个随机移动障碍的后续迭代只需要48ms± 4 ms。注意，这条轨迹比之前的图5中要长得多。
以TEB与STOMP([16])在同一场景下的计算量为基准(图8)。STOMP最初是针对机械臂的，因此将待优化的关节变量替换为平面状态x和y。STOMP不依赖梯度信息或要求可微分的目标函数。相反，g20框架用数值逼近梯度，要求目标函数可微。我们的STOMP实现使用了20个随机轨迹来执行一个更新步骤。
该实现包含所提出的加速度矩阵和障碍代价函数。最初，STOMP的目标是实现一个无碰撞的轨迹，不一定是一个快速的轨迹。显然，这对于移动机器人的运动规划是不够的。有可能增加更多的状态和目标。然而，即使只考虑无碰撞路径的STOMP-2D，他也被TEB超越。

图8(c )显示了所有12个初始轨迹的STOMP优化结果。每个轨迹由80个2d点组成，对应于每个TEB的平均位姿数(TEB采用动态调整尺寸)。位姿的固定数量与加速度矩阵的权重相结合会影响任务持续时间。对于不同的情况，必须调整权重(见图8©中较长的轨迹)。利用所提出的目标函数，我们的STOMP实现无法处理TEB主要处理的不连续初始轨迹。将运行时间与图7(b)中的TEB进行比较。TEB执行优化的速度明显快于STOMP。注意，STOMP参数是直观地选择的，因此它们在很大程度上可与2D情况下的TEB相比较。此外，我们还实现了一个二维版本的CHOMP(没有Hamilton Monte Carlo)进行比较，但得到的结果在上述场景中不具有足够的空间分辨率。

B 机器人实验
图9：TEB适应性实时避障

利用时间信息增强弹性带，使用g2o框架，实现机器人的实时轨迹调整和控制。图9显示了一个真实机器人实验的一系列快照，在这个实验中，一个人走过这个场景(类似于之前模拟的只有一个障碍的场景)。TEB实时适应原机器人轨迹(t = 0)，在t∈[6,12]区间内，通过变形原轨迹远离障碍物，避免了即将与人发生碰撞。

5 结论与进一步工作

本文介绍了用TEB实现实时轨迹修正的数值方法，重点介绍了g20框架下的实现方法。TEB的创新之处在于用时间信息来扩充经典弹性带。因此，不仅可以考虑与路径有关的几何和运动学约束，而且可以同时考虑移动机器人的动力学约束。G2o为稀疏系统结构提供了算法和求解器。我们在本文中证明了TEB具有这样的稀疏系统结构，因此可以用g2框架有效地解决它。该算法实时运行，从而直接为底层机器人运动控制器生成命令。该方法具有较高的灵活性，易于适应不同的机器人运动学和应用要求。

未来的工作将致力于运行时的进一步改进。第一种方法是为优化算法提供解析雅可比矩阵。二是利用稀疏TEB系统矩阵的a-先验排序从一个优化周期到下一个优化周期。三是根据规划区间，动态调整TEB在时间间隔长度和模型细节度上的分辨率。对于机器人运动控制，只有接下来的几个状态是相关的，因此遥远未来的远程配置是在一个粗糙的规模上规划的。

该方法的一个更基本的改变是切换到稀疏约束优化框架。这使得现行的惩罚功能限制的提法过时了。

感谢

这项工作得到了作为R3-COP项目一部分的ARTEMIS联合事业和德国联邦教育和研究部(BMBF)的资助，批准号为no. 01IS10004E。

参考文献

[1] S. M. LaV alle, ”Planning Algorithms”. Cambridge University Press,
Cambridge, U.K., 2006.
[2] S. Quinlan, O. Khatib, ”Elastic Bands: Connecting Path Planning and
Control”, in Proc. of the IEEE Int. Conf. on Robotics and Automation
(ICRA), pp. 802-807, 1993.
[3] S. Quinlan, ”Real-time modification of collision-free paths”, PhD
thesis, Stanford University, 1994.
[4] M. Khatib, ”Sensor-based motion control for mobile robots”, Labora-
toire d’Automatique et d’Analyse des Systèmes LAAS-CNRS, 1996.
[5] M. Khatib, H. Jaouni, R. Chatila, J. P. Laumond, ”Dynamic Path
Modification for Car-Like Nonholonomic Mobile Robots”, in Proc.
of the IEEE Int. Conf. on Robotics and Automation (ICRA), 1997.
[6] B. Graf, J. M. H. Wandosell, C. Schaeffer, ”Flexible Path Planning for
Nonholonomic Mobile Robots”, Fraunhofer Institute Manufacturing
Engineering and Automation (IPA), 2001.
[7] O. Brock, O. Khatib, ”Executing Motion Plans for Robots with Many
Degrees of Freedom in Dynamic Environments”, in Proc. of the IEEE
Int. Conf. on Robotics and Automation (ICRA), pp. 1-6, 1998.
[8] F. Lamiraux, D. Bonnafous, O. Lefebvre, ”Reactive path deformation
for nonholonomic mobile robots”, in IEEE Transactions on Robotics,
Vol. 20, No. 6, pp. 967-977, 2004.
[9] H. Kurniawati, T. Fraichard, ”From path to trajectory deformation”,
IEEE/RSJ Intl. Conference on Intelligent Robots and Systems (IROS),
pp. 159-164, 2007.
[10] V. Delsart, T. Fraichard, ”Reactive Trajectory Deformation to Navigate
Dynamic Environments”, in Proc. of the Second European Robotics
Symposium (EUROS), Vol. 44, pp. 233-241, 2008.
[11] B. Lau, C. Sprunk, W. Burgard, ”Kinodynamic Motion Planning
for Mobile Robots Using Splines”, IEEE/RSJ Intl. Conference on
Intelligent Robots and Systems (IROS), pp. 2427-2433, 2009.
[12] C. Sprunk et al. ”Online Generation of Kinodynamic Trajectories for
Non-Circular Omnidirectional Robots”, in Proc. of the IEEE Intl.
Conference on Robotics and Automation (ICRA), pp. 72-77, 2011.
[13] C. Sprunk et al., ”Improved Non-linear Spline Fitting for Teaching
Trajectories to Mobile Robots”, in Proc. of the IEEE Intl. Conference
on Robotics and Automation (ICRA), pp. 2068-2073, 2012.
[14] J. Mattingley, Y. Wang, S. Boyd, ”Receding Horizon Control: Auto-
matic Generation of High-Speed Solvers”, in IEEE Control Systems
Magazine, Vol. 31, No. 3, pp. 52-65, 2011.
[15] N. Ratliff et al. ”CHOMP: Gradient Optimization Techniques for
Efficient Motion Planning”, in IEEE Intl. Conference on Robotics and
Automation (ICRA), May 2009.
[16] M. Kalakrishnan et al. ”STOMP: Stochastic trajectory optimization
for motion planning”, in IEEE Intl. Conference on Robotics and
Automation (ICRA), pp. 4569-4574, May 2011.
[17] C. Rösmann et al. ”Trajectory modification considering dynamic
constraints of autonomous robots”, in Proceedings of the 7th German
Conference on Robotics (ROBOTIK 2012). May 2012.
[18] K. Konolige, ”Sparse Bundle Adjustment”, in F. Labrosse et al.,
editors, Proc. of the British Machine Vision Conference, pages 102.1-
102.11. BMVA Press, September 2010.
[19] R. Kümmerle et al., ”g2o: A general framework for graph optimiza-
tion”, in Proc. of the IEEE Intl. Conf. on Robotics and Automation
(ICRA), Shanghai, China, May 2011.
[20] P. R. Amestoy, T. A. Davis, and I. S. Duff, ”Algorithm 837: Amd, an
approximate minimum degree ordering algorithm.”, in ACM Trans.
Math. Softw. vol. 30, pp. 381-388, September 2004.
[21] Y. Chen et al., ”Alogithm 887: Cholmod, supernodal sparse cholesky
factorization and update/downdate”, in ACM Trans. Math. Softw. vol.
35, pp: 22:1-22:14, October 2008.
[22] L. E. Kavraki et al., ”Probabilistic roadmaps for path planning in high-
dimensional configuration spaces”, in IEEE Transactions on Robotics
and Automation, Vol. 12, No.4, pp.566-580, August 1996.