机器人控制算法—TEB算法文献阅读Integrated online trajectory planning and optimization in distinctive topologies

论文题目：Integrated online trajectory planning and optimization in distinctive topologies
独特的集成在线轨迹规划和优化拓扑

摘要：

本文提出了一种新的基于拓扑特征的移动机器人轨迹在线优化的集成方法。在线轨迹优化通过最小化路径长度、过渡时间或控制工作量等目标，使全局规划器生成的初始粗略路径变形。移动机器人的运动学运动特性和与障碍物的间隙对轨迹优化施加了额外的等式和不等式约束。当地规划者
通过仅将搜索空间限制为局部最优解来考虑效率。然而，目标函数通常是非凸的，因为障碍物的存在会产生多个不同的局部最优。
所提出的方法保持并同时优化不同拓扑的可容许候选轨迹的子集，从而在一组备选局部解中寻找总体最佳候选。获得了差速驱动和类车机器人的时间最优轨迹，通过采用时间弹性带方法来有效地解决潜在的轨迹优化问题。对各种示例场景的调查以及与传统当地规划者的比较分析证实了综合勘探、维护和优化地形独特轨迹的优势。
1. Introduction
在服务机器人和自主运输系统的背景下，移动机器人需要在高度动态的环境中导航，同时完成复杂的任务。在这种情况下，移动机器人的基本挑战之一是制定普遍适用的移动规划策略。在线规划优于离线方法，因为它们在运行时会立即响应环境的变化或机器人运动的扰动。众所周知的弹性带方法[1]在线局部变形路径。
预定义的内力收缩路径，而外力保持与障碍物的分离。[2]中提出了一种基于优化技术的替代建立路径规划方法。然而，传统的路径规划没有明确地结合运动的时间和（kino-）动态方面，因此忽略了具有有界速度和加速度的运动学或动力学运动模型施加的约束。Kurniawati等人将弹性带方法扩展到在线轨迹而非路径的变形[3]。该方法由两个阶段组成，第一阶段将离散轨迹点从障碍物中排斥，第二阶段加强动力学运动模型的连通性。Delsart等人将两个阶段合并为一个单独的操作[4]
目标优化通常试图最小化控制力、控制误差或起始姿势和目标姿势之间的过渡时间。然而，找到最优解所需的计算负担很高，这限制了其与反馈控制的直接在线集成。由于这一观察结果，许多研究人员正专注于有效地获得潜在轨迹优化问题的解决方案甚至近似值。动态窗口方法（DWA）是一种广泛应用于移动机器人导航的方法[5]。模拟轨迹是从速度搜索空间中重复采样的受一组可行速度的限制。成本函数评估每个候选轨迹w.r.t.到目标的剩余距离，前进速度和与障碍物的分离。最低-选择成本解决方案来控制机器人。方法通过限制有效trajec的可行集来说明效率-Tors到具有常数段的采样候选者的子集速度仅导致次优轨迹。Lau等人[6]和Sprunk等人[7]优化由样条曲线表示的轨迹，根据机器人的动力学约束连续地进行。一种基于协变梯度的在线规划算法下降法在[8]中提出。在之前的作品中，作者对弹性带进行了进一步的扩展，称为Timed elastic-频带（TEB）方法[9，10]。TEB有效地优化了机器人的轨迹w.r.t.（kino-）动态约束和非完整运动学，同时明确地结合了时间信息，以便在最短时间内达到目标姿态。该方法通过利用潜在问题表述。它已被推广为一种有效的动态系统时间最优模型预测控制方法[11]。
在实践中，由于计算资源有限，通常使用局部优化技术进行在线优化，但不能保证全局最优轨迹的发现。特别是在移动机器人导航中，由于障碍物的存在，往往会出现局部极小值。Kalakrishnan等人应用随机下降法来部分克服这些限制[12]。然而，该方法需要对轨迹进行大量采样，以估计全局最优的真实梯度。Kuderer等人[13]提出了一种两阶段局部优化方法，该方法生成一组可供选择的拓扑上独特的轨迹。所提出的方法从修改的Voronoi图中提取多个候选轨迹，这些轨迹通常与优化问题的局部极小值一致。属于同一等价类的路径根据每个障碍物的缠绕数通过等价关系进行分组。本文研究了一种在在线轨迹规划中过滤不同候选轨迹的相关方法。它主要不同于[13]w.r.t.采样策略、等价关系和轨迹优化技术。
探索拓扑上独特的路径和轨迹的想法并不新颖。然而，过去的方法主要关注全球离线路径和轨迹规划。[14]和[15]中介绍了使用不同等价关系操作的概率路线图（PRM）方法。所提出的算法原则上能够在大型复杂环境中识别复杂路径，但依赖于等价关系的算法而不是封闭形式的计算。我们的方法采用了计算上更有效的采样策略和等价关系。它旨在在环境的局部子区域中运行，因此适用于在线轨迹优化。显然，这并不能取代在大型环境中进行全球规划的需要。Knepper等人提出了一种基于路径采样和Hausdorff度量的局部规划器作为等价关系[16]。规划器执行离散路径选择，而不是使连续轨迹变形。Bhattacharya等人[17]提出了一种源自复分析领域的等价关系。封闭形式的解决方案激发了它在我们的轨迹滤波方法中的应用。Pokorny等人提出了一种基于单纯复合体的过滤[18]。

这一贡献提出了一种集成的在线轨迹规划方法，该方法结合了在运行时对多个可接受的拓扑上独特的轨迹的探索和同时优化。作为[19]的扩展版本，在线探索策略与TEB方法完全集成，提供了潜在的轨迹优化。因此，TEB方法在其原始描述[9]的基础上完全重新制定并进一步扩展到更通用的障碍物表示，支持机器人的前后运动，并在原始提案中考虑的差速驱动机器人之外完成类车机器人的导航任务。组合和积分方法保留了每个等价类的局部最优轨迹，以便于从以前的解决方案开始进行热启动。候选轨迹被重复采样，并且不仅在空间上而且在时间上进化，这显著减少了样本数量。将基于样本的方法与从Voronoi图中系统生成路点的方法进行了比较，以考虑候选轨迹的完整性和计算效率。所提出的方法可用作开源C++代码，并集成到ROS[20]中。
本文组织如下：
第2节制定了局部TEB优化方法，然后在第3节中探索不同拓扑中的轨迹。第4节描述了轨道勘探、规划和维护的整体整合。第5节演示并评估了差速驱动和车载移动机器人的真实模拟结果。综合规划方法与上述最初的本地运营TEB和DWA进行了比较。最后，第6节总结了结果，并对未来的工作进行了展望。

2. Timed-elastic-band approach

本节描述了在总体规划任务中执行实际轨迹优化的TEB方法。与[9]中最初提出的配方相比，该方法是完全重新制定和扩展的。

2.1定义和表述

设$s_k=[x_k，y_k，{\beta }_k{]}^{⊺}\in R2\times S1$表示离散机器人姿态
时间点$k$、$x_k\in R$和$y_k\in R$表示机器人的平面位置，${\beta }_k\in S^1$表示机器人的方向。离散轨迹是根据机器人姿态的序列$S＝S_k|k＝1，2，\dots ，n$来描述的。$TEB$用严格正的时间间隔$∆T_k\in R+，k=1,2，\dots$来扩充轨迹表示$n-1$。每个时间间隔表示机器人从姿势$s_k$过渡到其后续姿势$s_{k+1}$所需的时间。具有三个姿势的示例轨迹如图1所示。将姿态和时间间隔合并到参数向量$b\in b$中：

2.2.开环优化问题

TEB开环优化任务是找到控制，以便在最短时间内将机器人从初始姿态$s_s$转换到最终姿态$s_f$，同时满足动力学约束并保持与障碍物的安全分离。整个任务被表述为一个非线性程序：

T的最小化是不适定的，因为单个$∆T_k$是不受约束的。相反，时间均匀性是通过最小化∆T k的平方和来实现的，这强制了均匀的时间间隔$∆T$ $k=T/n$。初始姿态$s_1$和最终姿态$s_n$分别受到$s_s$和$s_f$的约束,两个连续姿态$s_k$和$s_{k+1}$之间的运动学约束在-由等式约束$hk（s_{k+1}，s_k）$合并。此外，对于类车机器人，必须满足最小转弯半径，该半径由$rk（s_{k+1}，s_k）$表示。机器人的速度和加速度分别受到不等式$v_k（\cdot ）$和${\alpha }_k（.）$的限制。不等式$ok（s_k）$维持了与障碍物的最小距离。每个术语在下面的段落中都有更详细的描述。

非完整运动学

等式约束$v_k（\cdot ）$强制遵守移动机器人的运动学约束。本文特别研究了只有两个局部自由度的差速驱动和类车机器人，因此机器人只能执行由直线段和圆弧段组成的平滑路径。根据[9]，非完整约束由几何解释定义。两个连续的姿态$s_k$和$s_{k+1}$需要位于恒定曲率的公共弧上，如图2所示。姿势$s_k$和方向$d_{k,k+1}=[x_{k+1}-x_k，y_{k+1}-y_k，0]⊺$之间的角度${\theta }_k$必须等于连续姿势$s_{k+1}$处的相应角度${\theta }_{k+1}$：

备注：公式4是由公式3推导出的，而公式3由几何关系可推出。而（4）中使用了向量的叉乘，$(cos{\beta }_k,sin{\beta }_k,0）$是机器人前进方向单位向量，叉乘正好是z轴的向量，即为航向角度，${\beta }_i$ 为机器人在第i段弧段相对于世界坐标系的绝对姿态。


*注意：向量的点乘$AXB=-BXA.$
方程（5）适用于能够在适当位置旋转的差速驱动机器人。对于汽车式机器人和阿克曼驱动器，机器人的运动进一步受到连续姿势之间最小转弯半径的限制。绝对转弯半径r由下式给出：

为了满足类车机器人的最小转弯半径，引入了$r_k（s_{k+1}，s_k）=r（\cdot ）-r_{min}$的不等式约束，$r_{min}$表示转弯半径的下界。对于差速驱动机器人，它变为$r_{min}$=0。

Limited velocity and acceleration

限制速度对于实际应用的成功至关重要。在没有任何约束的情况下，根据（2）得出的时间最优解意味着$∆T_k\to 0\forall k为v_k\to \infty$. 为了简单起见，考虑了每个姿态$v_k$和${\omega }_k$（例如机器人中心）的平移和旋转速度，而不是单个车轮的速度。这种简化对于大多数实际应用来说已经足够了。然而，该方法允许限制可以用$v_k$、${\omega }_k$和专用机器人设计参数表示的任意速度。平移速度和旋转速度是根据欧几里得方程使用有限差来近似的。两个连续姿势$s_k$和$s_{k+1}$之间的角距离：

注意，域S中的角度相减和相加在实际实现中需要归一化。$\gamma （s_k，s_{k+1}）$表示一个函数，该函数提取机器人向前或向后移动时平移速度的符号。对于分别限于沿直线段和圆弧段过渡的非完整机器人，方向投影的符号向量$qk=[cos{\beta }_k，sin{\beta }_k，0{]}^{⊺}$到距离向量$dk，k+1$上：

因此，运算符⟨·，·⟩应用标量乘积。由于许多常用优化算法不适用于非光滑函数，如（9），因此S形近似将投影映射到区间$[-1，1]$。我们的实施采用了：

变量$\kappa \in R+$表示改变斜率的比例因子（例如$\kappa =100$），等式（9）和（10）分别导致在$⟨q_k，d_{k,k+1}⟩=0$或$⟨q_k，d_{k,k+1}⟩\approx 0$时,速度消失和不正确。形象地说，如果姿势sk+1被放置为与姿势sk正交，就会发生这种情况。然而，这种配置不是前面提到的由非完整约束给出的可行集的一部分，并且对于两个姿态的位置部分重合的特殊情况，它是$d_{k,k+1}＝0\Rightarrow v_k＝0$。
$\pm v_{max}$和$\pm {\omega }_{max}$的极限速度是通过等式中的约束条件$vk（s_{k+1}，s_k，∆T_k）=[v_{max}-|v_k|，{\omega }_{max}-|{\omega }_k|]⊺$获得的。在负速度上的边界应该不同的情况下，可以对方程进行调整，为了简单起见，这里忽略了这一点，但在实际应用中通常更可取
类似的程序被应用于分别限制平移和旋转加速度ak和ωk。特别是对于k，它是：

为了清楚起见，$s_{k+2}、s_{k+1}$和s_k由它们的相关速度（7）代替。加速度的极限由不等式${\alpha }_k（s_{k+2}，s_{k+1}，s_k，∆T_{k+1}，∆T_k）=[a_{max}-|a_k|，{\omega }_{max}-|{\omega }_k|{]}^{⊺}$获得。在$k=1$和$k=n-1$时出现特殊情况，其中$V_1$和$v_{n-1}$分别由所需的起始速度和最终速度$（v_s，{\omega }_s）$和$（v_f，{\omega }_f）$代替。

Obstacle avoidance

机器人应该在不与障碍物发生任何碰撞的情况下到达目标。该方法适用于各种各样的障碍物表示。在假设姿态$s_k$和障碍物周边上的点集之间的最小欧几里得距离由连续函数描述的情况下，基础优化方法是可行的，并且在计算上是有效的。本文用点、圆、线和多边形的集合来考虑障碍物的表示。障碍物表示为R2中的单连通区域，并且表示为O。在存在R个障碍物O l的情况下，$l＝1，2，R$ 添加下标$l$。设$\rho （s_k，O）：R_2\times S_1\times O\to R$表示障碍物$O$和姿态$s_k$之间的最小欧几里得距离。在圆形机器人的情况下，$s_k$的定向部分${\beta }_k$可以忽略。不等式约束在所有障碍物和姿态$s_k$之间保持最小间距${\rho }_{min}$，根据：


显然，这个集合是非凸的，使得相应的非线性程序（2）表现出多个局部极小值。由于障碍物的存在而导致的局部极小值的出现是轨迹中的一种常见现象以及路径规划。障碍物的数量$l$对局部极小值的数量有显著影响。由于假定资源有限，在线优化首选局部优化方法，因此该解决方案在很大程度上取决于全局规划轨迹**$b$**，该轨迹作为局部优化器的初始解决方案。注意，对于大多数实际应用，优化过程中所需的距离计算数量可能会减少，这样每个障碍物$O_l$只影响附近姿态的子集，而不是整个姿态序列$s_k，\forall k$。在随后的采样间隔之间更新障碍物和附近姿态之间的关联，以在运行时完善次优解决方案。

2.3.近似最小二乘优化

求解具有硬约束的非线性程序在计算上是昂贵的。因此，提高快速在线求解器的效率已成为近十年来的一个重要研究课题。TEB方法进一步研究了无约束优化技术的应用，因为它们已经得到了很好的研究，并且在开源软件包中的成熟实现已经广泛可用。无约束优化避免拉格朗日响应。Karush–Kuhn–Tucker乘法器（对偶变量）导致Hessian矩阵的维数与$b$中原始变量的数量相同。
将精确非线性程序（2）转换为近似非线性最小二乘优化问题，该问题在求解器通过一阶导数近似Hessian并利用问题的稀疏性模式时得到有效求解。约束作为附加惩罚项被纳入目标函数。由于无约束目标函数仅由平方非线性项组成根据[21]应用惩罚函数。注意，存在其他无约束近似，如对数屏障、增广拉格朗日或精确惩罚方法[22]，但这些方法包含非平方项，因此不适用于无约束最小二乘解算器。
为了更好的可读性，以下省略了约束的参数。等式约束$h$表示为具有标量权重${\sigma }_h$和恒等式$I$的二次惩罚，通过：

不等式通过加权单边二次罚近似：

min运算符是按行应用的。进一步的不等式${\alpha }_k$和$o_k$以类似的方式近似。初始和最终约束$s_s$和$s_f$分别通过替换消除，因此不受优化影响。$∆T_k>0$隐含地由（7）和（8）中的差商合并，因为$∆T_k\to 0$的商发散（初始$∆T_k$必须为正）。具有目标函数的整体无约束优化问题$\tilde{V}（b）$ 近似（2）的值由下式给出：

变量b表示最佳参数向量。根据二次罚理论[21]，众所周知，在所有权重趋于无穷大σ的情况下，$b\ast$仅与非线性程序（2）的实际极小值一致$\sigma \to \infty$. 不幸的是，大权重引入了问题的病态特征，使得底层求解器由于步长不足而无法正确收敛。TEB方法放弃了真正的最小化器，转而使用用户定义的权重来获得次优但计算效率更高的解决方案。
适用于机器人内部的中小型杂乱环境感知领域我们的实验表明，单位权重1提供了一个合理的出发点，除了与非完整运动学的等式约束相关的权重${\sigma }_h$，它应该选择高几个数量级（≈1000）。建议使用此设置，因为与不等式约束相比，惩罚函数（13）较小，并且轨迹与机器人运动学的近乎完美的一致性至关重要。
非凸有符号速度约束（10）到二次惩罚项的变换引入了局部极小值（见图第3a段）。然而，图3b揭示了这些局部极小值在整体目标函数中被冲突的非完整约束所抵消（5）。

2.4.近似优化问题的求解

文献提出了大量用于解决非线性最小二乘问题的算法，例如（15）。常用的求解器是高斯-牛顿、Doglig或Levenberg-Marquardt（LM）算法[21]。TEB方法利用LM，因为它在鲁棒性和效率之间具有适当的平衡。LM构成了一种信任区域策略，只接受降低总体代价，优化问题在变为奇异的情况下被隐式正则化。应用该方法需要稀疏线性系统的解，其中$（H+\lambda I{）}^{-}1$是用阻尼因子λ计算的。$H=J^{⊺}J$表示本身依赖于Jacobian J的Hessian。开源图优化框架g2o[23]实现了用于求解（15）的LM的高效稀疏变体。请注意，b中姿势和时间间隔的顺序（见（1））会影响优化问题的结构。由于（2）中的目标函数和个体约束项仅取决于一小部分参数，因此得到的Hessian是稀疏的和带状的。

2.5. Closed-loop control

前几节中提出的优化问题是在运行时重复求解。在每个采样间隔内仅命令计划轨迹的第一控制输入机器人。这是模型预测中的一个常见过程考虑环境干扰和变化的控制-通过反馈。在我们的案例中，移动机器人由相对于其中心的平移和旋转参考速度旋转这些控制输入，特别是$V1$和$\omega 1$，是计算-分别根据（7）和（8）。此外，TEB方法支持热启动，以有效地改进先前获得的解决方案。在每个采样间隔内，开始和结束姿势被更新，并且先前的轨迹被重新采样w.r.t。其长度$n$，以考虑$∆T_k$的变化幅度，并引导规划器实现所需的轨迹时间离散化$∆T_{ref}$。如果$∆T_k$大于参考步长，则在$s_k$和$s_{k+1}$之间插入一个新样本，否则$s_{k+1}$将被移除[9]。调节样本数量时的小滞后$∆T_{hyst}$可避免过度振荡。实验结果是在$∆T_{ref}=0$的情况下获得的$∆T_{ref}=3s$和$∆T_{hyst}=0.1∆T_{ref}$参考。初始轨迹长度n是通过将初始轨迹均匀划分为具有0.3m等距空间间隔的分段线性段来获得的。在下文中，该方法被进一步扩展，使得在第4节中重新关注闭环控制。

3.探索独特的拓扑结构

以下各节讨论了求（2）resp（15）的全局最小值的问题。 。如第2.2节所述，障碍物的存在引入了多个局部极小值，因为由此产生的可行机器人姿态集是非凸的。因此，找到局部极小值与提取独特拓扑相吻合。所开发的方法旨在识别被占用的障碍物区域所涉及的N个相关拓扑，并提供初始轨迹$b_j,j=1，2，N$。这些初始轨迹旨在通过第2节所述的TEB方法并行优化。从一组备选方案中选择成本最低的轨迹，以揭示全局最小值。此外，将所开发的方法集成到闭环控制系统的状态反馈中，以在运行时探索和更新局部解集。

3.1等价关系：同伦类和同调类

在局部极小值是由障碍物引起的合理假设下，用于识别局部候选者的搜索空间被缩减到仅包含位置部分的2D平面$z_k=R（x_k，y_k）\in s_k$的$R^2$，因为它们不允许是
的一部分。参数${\beta }_k$和$∆T_k$不受直接影响。机器人位置序列的路径定义为$\tau =（z_k{）}_{k=1，2，..n}$ $b=h（\tau ）$表示从（初始）路径到操作时序参数b的映射。注意，由于轨迹或初始机器人向前或向后运动的解的对称性，全局成本函数可能具有多个局部极小值。在我们的情况下，初始局部解决方案由全局规划器生成的路径决定。
在不失一般性的情况下，在复域中，路径$\tau$的二维位置$z_k\in R^2$由$z_k=x_k+i{y}_k\in C$表示。这同样适用于嵌入$C$而不是$R^2$中的障碍物$O_l$。在下面的粗体脚本中，只要优先使用复杂的域表示，就会省略。所提出的方法是基于同并类的理论。根据[24]定义了第一个同位路径
定义1（同源路径）:
分别连接相同起始点和目标点$z_s$和$z_f$的两条路径${\tau }_1$和${\tau }_2$是同构的，当且仅当其中一条路径可以在不相交任何障碍物的情况下连续变形为另一条路径。所有路径的集合，彼此都是同宗的，表示为同宗类。
基于上述定义1，具有温启动的动力学问题的局部优化方法在后续优化步骤的解路径$\tau \ast$和$\tau$之间建立了一个同构。设$A=b_{\epsilon }\in b|\tilde{V}（b\ast ）\le \tilde{V}（b_{\epsilon }），b_{\epsilon }=b\ast +\epsilon$表示具有目标函数的局部极小值$b\ast$的（吸引）邻域$\tilde{V}（16）$的。形象地说，$vb\epsilon =h（{\tau }_{\epsilon }）\in A$的所有路径τε都收敛到相同的局部极小值，因此与相同的同伦类有关。每个同伦类A（i）的单个代表性b（i）ε∈A（i）提供了问题（15）的有效初始解，并且足以识别第i个同伦类的局部最小值b*（i）。
闭形式的同伦类的一般计算是困难的。[24]建议用同调类代替同伦论，因为它们更容易计算。同源类定义了一组同源路径，其中元素彼此同源。
定义2（同源路径）。分别连接相同起始点和目标点z s和z f的两条路径τ1和τ2是同源的，当且仅当τ1⊔-τ2形成嵌入C中的2D流形的完全边界，该流形不包含和相交任何障碍物。
同源性意味着同源性，但相反的含义不成立。然而，对于大多数实际的移动机器人规划来说，这两个定义可以被认为是等价的。图图4显示了一个例子，其中路径τ1和τ2属于两个不同的同伦类，因为它们不能在不相交任何障碍物的情况下连续变换为彼此。另一方面，τ1和τ2是同源的，因为不相交并集τ1⊔-τ2所跨越的区域A=A1ŞA2不包含障碍。它们属于同一同源类。注意，同源路径也可以通过在周期τ1⊔-τ2内每个障碍物处评估的消失绕组数来定义。
[24]和[17]提出了一种基于复数分析确定同源性类别的分析方法。总之，称为H-信号的同调不变量构成了一个等价关系，它为同一同调类的路径分配了一个唯一的复数。$H$信号最初分别为起点$z_s$和终点$z_f$之间的连续路径定义。设${\tau }_{(t)}$表示连续路径，使得${\tau }_{(t=0）}=z_s$和${\tau }_{（t=t）}=z_f$，复同调不变量由下式定义：

方程（18）直接来自柯西积分公式[17]的定义。显然，$F（z）$取决于障碍物的位置。[17] 建议以下函数$F（z）$称为障碍物标记函数：

$（{\xi }_l\subseteq O_l）\in C，\forall l=1,2，R$表示R障碍物的代表点。每个代表点ξl分别从障碍物区域内部的障碍物形状$O_l$中任意选择。$f_0$表示$C$上的任意解析函数。在我们的实验中，我们选择$f0（z）=ab（z-BL）（z-TR）$，其中$a=ceil（R/2），b=R-a$。运算符ceil（·）映射到以下最小的整数。
参数$BL\in C$和$TL\in C$表示环境的左下角和右上角。该公式与[17]中提出的公式略有不同，因为它提高了障碍物数量的可扩展性（在我们的实验中，$r=0-500$，典型的$H$信号值小于$10^{1}0$）。原始公式倾向于大的H信号$（>10^{1}00）$，即使对于可能发生数值不稳定性的数百个障碍物也是如此。
为了计算离散路径τ的H-信号，使用线段的（18）解析解。[17] 导出连接两点zk和zk+1的线段的解析表达式：


离散路径（由线段组成）的$H$信号通过以下公式计算：

注意，（20）的实际实现需要复对数理论。[24]建议选择使$z_{k+1}-{\xi }_l$和$z_k-{\xi }_l$之间的角度最小化的分支（通过测试一些接近零的值$\alpha \in Z$）：

所提出的H-信号确定多个路径是否属于相同的同调类，如果所有H-信号在数值精度上都相同，则满足该同调类。

3.2. Discovery of homology classes

基于上一节中提出的同调不变量，开发了一种探索相关同调类的算法。[17] 引入了一个搜索图，该搜索图由H-信号扩充，以便将轨迹限制在给定的可容许同调类集合中，同时调用a*-搜索来寻找最优轨迹。相比之下，我们的方法并没有试图通过图搜索直接解决规划问题。相反，它使用局部在线优化方法求解（15）。这种修改需要持续维护当前的同源性类别，并结合潜在的轨迹优化来发现新的同源性类。所需的计算时间是特别重要的，因为解决诸如（15）的非线性问题仍然需要大量的计算工作。基于上一节中提出的同调不变量，开发了一种探索相关同调类的算法。[17] 引入了一个搜索图，该搜索图由Hsignatures扩充，以便将轨迹限制在给定的可容许同调类集合中，同时调用a*-搜索来寻找最优轨迹。相比之下，我们的方法并没有试图通过图搜索直接解决规划问题。相反，它使用局部在线优化方法求解（15）。这种修改需要持续维护当前的同源性类别，并结合潜在的轨迹优化来发现新的同源性类。所需的计算时间是特别重要的，因为解决诸如（15）的非线性问题仍然需要大量的计算工作。因此，所提出的方法收集了粗的、无碰撞的候选轨迹，这些轨迹的连续路点仅在向前方向上排列。H信号被用作滤波器，以消除每个同源类的除一个轨迹外的所有轨迹。
给定机器人的当前位置$z_s$、目标$z_f$（均采用C表示法）和障碍物区域集$O=O_l|l=1，2，\dots ，R$，构建探索图$G=V，E$，以收集可容许路径的初始子集。顶点集由$V=z_s，{\zeta }_i，z_f\in C|{\zeta }_i\in O，i=1,2，\dots ，i$定义。${\zeta }_i\in C$是以后可能成为轨迹一部分的航路点样本。

3.2.1Exploration based on Voronoi diagrams

从环境的Voronoi图生成具有所有可行候选轨迹的完整探索图。给定距离度量（例如欧几里得距离），Voronoi图根据障碍物的数量$l＝1，2，\dots ，$将2d平面划分为多个区域$R_l\subseteq C$,R.每个区域$R_l$定义了（严格地）比所有其他障碍物$O$\ $O_l$更靠近相应障碍物$O_l$的点集。形式上，Voronoi图Gvd被定义为所有区域边界的集
为了将$G_{vd}$变换为勘探图G，在空间上离散集合$G_{vd}$，使得离散点${\zeta }_i，i=1，2，I$被认为是顶点，并且两个连续顶点之间的边界的相应部分被包括为双向边。此外，起始位置和目标位置$z_s$和$z_f$分别连接到从Voronoi图获得的顶点。开始和目标的合理策略是找到最接近的点${\zeta }_i$，每个点可以生成无碰撞线段的边。从Voronoi图获得的示例环境的探索图G如图5所示。

基于生成的图G，通过由访问列表扩充的深度优先搜索来提取其在$z_s$和$z_f$之间的简单路径。根据（21）计算每个路径的H-签名，并在其还不是成员的情况下将其添加到已知签名集。具有重复H签名的候选路径将被丢弃。尽管Voronoi图已经提供了独特的拓扑结构，但需要H信号来确定之前发现的和优化的轨迹，如以下第4节所述。
将查找所有简单路径和过滤同源路径这两个步骤组合到一个单独的搜索算法中，以提高效率。算法1构成了修改的递归深度优先搜索。集合$L$包含那些已经访问过的顶点，使得在达到目标之后，$L$由从$z_s$到$z_f$的完整路径候选者组成。该候选路径与行10和11中$H$中的潜在同源物重复匹配。如果其同源类是新颖的，则从路径L初始化底层优化问题的对应轨迹。对{$z_s$，${\zeta }_i$，$z_f$}定义的粗路径进行子采样，并根据后续位置之间的方向初始化姿态的定向部分${\beta }_k$。不是存储轨迹的2D位置部分$\tau$，而是存储完整的优化参数$b$，用于优化的后续热启动。为了限制计算时间，在第2行指定了最大区别拓扑的数量。

3.2.2. Sampling-based exploration strategy

作为完整探索的替代方案，我们提出了一种生成探索图G的采样策略。之前提出的基于Voronoi图的方法是完整的，但生成候选轨迹的计算时间与障碍物的数量不成正比。在有限计算资源下的在线轨迹规划环境中，人们倾向于更快的计算来识别最有希望的候选轨迹的子集，而不是探索完整的轨迹集。
航路点采样遵循概率路线图（PRM）方法[25]。因此，从预定义区域$Q\subseteq C$均匀地对路点${\zeta }_l$进行采样。在获得所需数量的样本之前，拒绝与任何障碍物区域相交的采样路点。边缘集合E是由航路点种子构建的。如果以下条件适用，边连接一对顶点$w_1\in V$和$w_2\in V$：

（方向相对于目标方向是向前的）
显然，第一个条件消除了那些到目标的欧几里得距离没有单调递减的路径。然而，在线轨迹规划人员通常从全局规划人员那里获得他们的中间目标。假设这些子目标被正确地排列和排序，则对非循环图的限制是合理的。这种条件显著减少了候选轨迹的数量和图搜索的计算工作量。可以增加正向角宽度的阈值$\theta$，以进一步缩小视线。其余步骤与上述算法相同。如图6所示，$O_1$或$O_2$内部的样品被拒收。显然，计算时间随着样本数量的增加而呈指数级增加。在每次迭代中重复采样，使得在稍后阶段仍可能发现新的同源类。有可能构建违反这一假设的人工环境。然而，在大多数实际应用中，该假设对于轨迹优化所考虑的机器人局部环境是有效的。有关相应的分析，请参阅第5节。

4. Integrated online trajectory planning in distinctive topologies

以下部分描述了机器人控制反馈回路中同源类发现与TEB方法的集成。算法2执行在移动机器人的每个采样间隔调用的主要规划步骤。在初始调用期间，容许轨迹的集合$\Gamma$是空的（在实际实现中$\Gamma$可以包含由全局规划器提供的初始轨迹）。根据第3.2节，通过在感兴趣的区域（通常是连接$z_s$和$z_f$的矩形或半圆）中播种随机样本，在第7-9行中创建了一个新的图形。然后，应用改进的深度优先搜索（参见算法1）以发现不同的同源类。$\Gamma$中的每个类都初始化了具有唯一H-签名的单个代表性轨迹。候选轨迹$\forall b_i\in \Gamma$在第11-14行中通过应用$I_{teb}$迭代来最小化（16）而同时优化。此外，如第2.5节（第13行）所述，根据参考时间离散化对每个轨迹进行重新采样。根据最小目标函数值（16）选择全局最优轨迹$b^{\ast }$。控制变量$v_1$和${\omega }_1$分别通过应用（7）和（8）从$b^{\ast }$中获得。

在随后的规划步骤调用中，Γ和H已经包含在先前迭代中优化的候选轨迹，因此执行第3-6行。根据新的机器人状态和感知，更新当前的起始姿态$s_s$、初始速度$（v_s，{\omega }_s）$和最终姿态$s_f$。此外，如果有多个备选方案，则验证当前轨迹集的可采性（第5-6行）。特别是验证了第3.2.2节的边缘条件。在违反的情况下，轨迹和相应的H信号被消除。保持至少一个不满足所有边缘要求的单个轨迹（第5行）确保机器人必须向后行进的轨迹不会被拒绝（例如，通过将全局计划保留为候选）。如果障碍物配置发生变化，则更新当前H信号集（第6行）.

5. Experimental results and examples

以下示例和场景展示了所提出的在线轨迹规划方法的能力。算法是在一台运行Ubuntu的机器上执行的，该机器配备了3.4 Ghz的英特尔i7 CPU。C++源代码可在线获得[20]。
首先，研究了在具有随机产生的障碍物的中等尺寸环境（10m×10m）中寻找替代轨迹的过程，并分别研究了机器人的起始目标位置。针对所提出的探索策略，特别是Voronoi图方法和具有不同样本数量的基于采样的方法，分析和比较了每个生成场景的探索结果。因此，考虑了三种不同类型的障碍物表示：（I）点状障碍物，（ii）线形障碍物和（iii）两者的混合，但具有离散线（0.1m步长），以便基于实际导航场景中经常出现的激光测距数据模拟地图表示。对于每种类型，生成100个环境，使得障碍物之间的最小距离至少为1.5m，并且线段不长于4m。此外，通过维护先前找到的候选集，每个探索策略都会为每个环境调用多次。因此，研究了在线探索过程中揭示新的候选轨迹的能力。采样区域Q被选择为机器人的当前起点zs和目标zf之间的定向矩形。它的宽度为6米，长度扩展了|zf−zs|的10%，以便在接近起点和终点的地方进一步采样。每个边缘相对于球门的最大视线设置为θ=π/3。相比之下，Voronoi图被限制为与边界框相同的矩形，但不限制视线。Voronoi图是基于[26]中算法的有效实现生成的，作为boost开源的一部分.在调用算法1之前，会拒绝分离两个接近障碍物的边缘（间隔小于机器人的范围），以防止规划器生成不可行的轨迹，并减少将激光测距数据和点云作为障碍物表示的场景的计算时间.