【泡泡航行天下】用于无人机在线重规划的时间连续轨迹优化方法（IROS）

其中多项式系数为：

给定这个轨迹的表达式，接下来我们寻找系数集p*来最小化目标函数J。和CHOMP算法类似，我们的目标函数包含两个部分：一部分尝试最小化导数D，Jd，另一部分尝试最小化与环境的碰撞，Jc。

在接下来的部分中我们将会介绍对目标成本Jd和Jc的选择，优化方法以及地图表示来解决这个实时问题。

目标Jd可以通过以下公式来进行计算：

其中R是增广成本矩阵，Rxx表示该矩阵中的相应的块。

Jd的雅可比关于参数向量可以计算为:

然后，碰撞成本Jc是下面的先积分，在每个段m上进行积分（其中tm是段的结束时间）

其中c(f(t))是势场成本函数。最后，使用乘积和链式法则，我们获得每个轴k的雅可比行列式：

我们使用启发函数来估计段时间tm，以满足动力学约束，并且我们在优化期间保持这些时间固定。

2. 地图表示

地图表示和势场成本函数的选择是上述算法的核心。当然，势场成本函数必须是平滑的，但其梯度也必须能够将轨迹推离碰撞。我们使用[4]中描述的势场，它是欧几里德有符号距离场（ESDF）值d（x）的函数。

我们使用基于体素的地图表示方法，因为它们可以快速构建和保持。为了确保轨迹不会和环境发生碰撞，我们必须沿着轨迹检查每个体素。请注意，我们的连续时间方法仍然具有优于离散时间方法的优势，因为我们可以灵活地对碰撞轨迹进行采样，并且可以在迭代之间更改此间隔。我们选择沿着与地图体素分辨率相等的每个弧长点Δs来评估函数。这显着加快了计算速度，而不会影响安全性。

3. 优化方法

在任何非不平常的环境中，（3.）中的优化问题都可能是非凸的、高度非线性的。为了最小化函数，我们选择使用类似于BFGS 的拟牛顿方法（尽管也可以使用其他更简单的方法，如梯度下降）。

然而，使用这种方法找到的所有解决方案都是固有的局部解决方案，并且根据初始化，它们很容易陷入局部极小值。因此，为了增加找到可行（非聚集）解的机会，我们做了几个随机重启，用一个随机量扰动初始状态，然后选择成本最低的轨迹作为最终解。[9]对局部轨迹优化随机重启的必要性进行了更深入的讨论。

4. 重规划系统

在这一部分，我们将介绍一个完整的系统，该系统可以在线动态更新地图数据，实时运行局部重规划方法。首先，我们介绍如何构建地图及其伴随距离场，然后我们讨论使用全局规划作为局部规划器的输入，最后，如何选择开始和结束点进行重规划。

4.A 增量建图

如第上文所述，我们需要欧几里得有符号距离场（ESDF）来计算碰撞势场。我们的地图表示是一个Octomap，它包含三种状态之一的体素：自由、未知或被占用。

在首次构建地图时，我们会填充所有单元格的占用情况并计算完整地图的距离，这在计算上是昂贵的。为了使我们的算法能够实时运行，我们在Octomap表示中跟踪已更改的节点，并使ESDF中的所有体素无效，这些体素将这些节点作为父节点（即，不同状态的最近邻居）。这允许我们重新计算每次地图更新仅几十到几百个体素的距离值，而不必重新计算数百万个体素的完整密集网格。

关于地图表示的一个关键点是，尽管Octomap允许三个具有完全概率的状态（自由、未知和被占用），但为了构造一个距离场，我们必须离散到两个状态-自由和被占用。如何处理未知体素是一个安全问题：除非机器人能够在已知的自由空间中停留，否则我们无法安全地规划并通过它们。因此，我们选择将未知视为被占据，创造一个非常保守的规划。

4.B 全局规划器

接下来，我们在原始Octomap中构建了一个全局规划，并将未知空间视为自由空间。这就创建了一个乐观的规划器，而局部的重规划是保守的，因此更安全。然后，该规划将会被用作重规划的先验，并允许我们使用未完成的重规划-----即如果局部轨迹优化未找到解决方案，我们只需停下来等待全局规划器找到新的路径。

我们的全局规划器具有两个阶段：首先，我们使用Informed RRT*找到一个拓扑上可行的直线路径，然后，通过它规划一个动态可行的多项式轨迹。

4.V 局部规划器

为了执行局部重规划，我们从全局规划（如果可用）或直线规划开始，作为先验的下一个航点，并逐步更新ESDF。

然后，我们为重规划算法选择合适的起点和终点。作为起点，我们选择当前轨迹上未来tR秒的点，其中tR是重规划器的更新率。由于我们的规划器即使在低阶导数下也能实现连续性和平滑性，因此我们能够在起点使用无人机的全状态，包括速度和加速度，即使在规划改变时也能保证一条平稳的路径。选择目标点作为全局轨迹上的一个点，该点位于起点之前几米，其中h是规划时域。如果未被占用，我们接受该点作为目标，否则我们尝试在ESDF中找到最近的未占用邻居，并且作为最终后备点，我们缩短规划时域直到找到自由目标点。

然后我们可以在这两点之间运行局部优化过程。优化成功找到无碰撞路径，或者我们尝试随机重启，直到找到无碰撞轨迹或飞行器停止并等待全局规划器选择新路径。

图一：我们的算法和CHOMP算法的2D评估对比结果。（a）：与CHOMP相比，我们的算法生成的典型路径具有不同的段数。只有一个段数时（红色），没有足够的自由度来避开所有障碍，但它能够找到一个有5个段数的解决方案（不同于CHOMP）。势场成本地图为灰色，原始障碍物边缘为蓝色。（b）：不同局部规划器的成功率与环境密度的关系。随着密度的增加，成功率会降低，但对于较少的段数，成功率也会降低，上述成功率的降低可以通过进行10次随机重启来抵消。（c）：按段数划分的成功规划的比例。这也显示了随机重启可以显著提高成功率，这使得算法可以避免发生碰撞的局部最小值。

图一，我们的算法和CHOMP算法的2D评估对比结果

图二：森林场景仿真评估。森林场景大小为10×10×10米，密度为0.2树/m2。路径规划在空间中的两个随机点之间，相距至少4米。黄色是使用我们的规划方法，青色是Informed

RRT* 和多项式优化方法（在全局规划部分中讨论），紫色是CHOMP规划方法。

图二，森林场景仿真评估

图三：这里我们展示了我们的局部重规划方法（青色 - 67.9米）与具有整个地图先验知识的全局规划方法（黑色 - 67.5米）的比较。环境尺寸为50×50米，密度为0.1树/m2，为清晰起见，我们仅显示障碍物的树干。局部重规划算法以4 Hz的速率运行，而全局规划器使用带有多项式平滑的 Informed

RRT *，在30秒内运行的结果。

图三，我们的局部规划器与全局规划器对比结果

图四：现实世界局部规划器的实验效果。原始目标点嵌入在第二个障碍物（粉红色）中，从起始位置看不到。随着时间的推移，路径不会与障碍物发生碰撞，最终路径（蓝色）是比实验早期许多规划路径更短、曲率更低。

图四，真实环境下局部规划器的实验效果

表一：该表格显示带有多项式平滑的RRT变体、CHOMP算法以及我们对一组90个森林规划问题的方法的比较，如图二所示。我们比较成功率、归一化路径长度（路径长度除以直线路径长度的解）和计算时间。可以看出，添加随机重启显着提高了成功率，但代价是计算时间较长。RRT *和RRT Connect能够解决更高比例的问题，但代价是性能降低。

表一，RRT变体、CHOMP算法以及我们的算法比较

表二：从真实环境实验种获得的完整重规划系统的时间。我们给出了整个实验的平均时间，这就是为什么总优化时间短于10次重启的最大时间（大多数规划迭代在没有任何重的情况下找到了一个可行的解决方案）。梯度时间给出了100多个成本函数的评估。*初始地图创建仅运行一次，不包括在总数中。

表二，

Abstract

Multirotor unmanned aerial vehicles (UAVs) are rapidly gaining popularity for many applications. However, safe operation in partially unknown, unstructured environments remains an open question. In this paper, we present a continuoustime trajectory optimization method for real-time collision avoidance on multirotor UAVs. We then propose a system where this motion planning method is used as a local replanner, that runs at a high rate to continuously recompute safe trajectories as the robot gains information about its environment. We validate our approach by comparing against existing methods and demonstrate the complete system avoiding obstacles on a multirotor UAV platform.

泡泡机器人SLAM的原创内容均由泡泡机器人的成员花费大量心血制作而成，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！

商业合作及转载请联系liufuqiang_robot@hotmail.com返回搜狐，查看更多