入选Science机器人子刊：机器人如何更聪明地躲避障碍？凸集图给出轨迹规划最优解

点击下方 卡片 ，关注 「3D视觉工坊」 公众号
选择 星标 ，干货第一时间送达

来源：深蓝AI

添加小助理：cv3d001，备注：方向+学校/公司+昵称，拉你入群。文末附3D视觉行业细分群。

扫描下方二维码，加入「 3D视觉从入门到精通 」知识星球，星球内凝聚了众多3D视觉实战问题，以及各个模块的学习资料： 近20门秘制视频课程 、 最新顶会论文 、计算机视觉书籍 、 优质3D视觉算法源码 等。想要入门3D视觉、做项目、搞科研，欢迎扫码加入！

导读：

当环境中有很多障碍物时，传统的轨迹优化方法（如基于采用规划器的方法）往往表现不佳。本文提出了一种新的基于凸优化的运动规划方法，该方法结合了贝塞尔曲线与凸集图（GCS）的最短路径优化框架。通过这种方法，研究人员将轨迹规划问题转化为一个混合整数优化问题，能够在快速解决高维空间中的问题。 ©️【深蓝AI】编译

机器人运动规划，尤其是在复杂的环境中设计无碰撞的轨迹，一直是机器人学领域的一个核心挑战。运动规划不仅涉及在高维度的空间中设计路径，还必须同时考虑机器人本身的运动学和动力学约束。现有的轨迹优化方法可以对简单的问题进行有效求解，然而，当机器人所在的空间充满障碍物时，轨迹优化问题通常会由凸优化转换为非凸优化问题，因此求解会变得非常复杂，导致很多传统方法失效。

为了应对这一难题，研究人员常常转而使用基于采样的运动规划算法，这类算法在复杂的配置空间中具有一定的优势，因为它们能够通过采样来找到可行的路径，并且具有概率完备性，意味着只要可行路径存在，它们理论上总能找到。然而，这些算法在处理高维空间的连续微分约束时表现不佳，生成的轨迹往往不是全局最优的，并且需要大量的采样和计算时间。在更复杂的动态系统中，轨迹优化与高层次的图搜索方法结合使用，虽然可以部分改善这一问题，但这些多层架构并未提供一个统一的优化框架。

基于这些背景，混合整数凸优化方法（MICP）为运动规划问题提供了新的思路。MICP方法能够结合基于采样的算法的完备性以及轨迹优化方法处理运动学和动力学约束的能力，并且可以提供全局最优解。然而，MICP方法在计算复杂性上存在很大的局限性，其求解时间通常非常长，即便是在小规模问题上也需要数分钟才能生成轨迹。

在这个背景下，研究者们致力于寻找一种新的方法，既能够应对复杂环境下的高维运动规划问题，又能快速地生成高质量的避障轨迹。文章基于此背景提出了一种新的运动规划算法，能够有效地解决这些问题，并在很短的时间内生成全局最优的避碰轨迹。

该研究所要解决的问题是为机器人设计在障碍物环境中无碰撞的连续运动轨迹。具体来说，研究者将此问题抽象为一个在无限维轨迹空间中的优化问题。研究者将将围绕障碍物进行规划的问题视为在一组“安全”区域内导航的问题，准确的说，机器人运动的无碰撞配置空间 被分解为一系列“安全区域” 。

在此基础上，研究者的目标是找到一条花费总时间 的安全轨迹 。在求解优化问题的过程中，设定求解目标是轨迹持续时间 ，轨迹长度 和能量 的加权求和，即 。

该优化框架的目标是通过凸优化和图论相结合的方法，解决机器人在障碍物环境中的无碰撞运动规划问题。本文提出了将运动规划问题表述为凸集图中的最短路径问题（SPP），并通过最近提出的技术将该问题转换为一个混合整数凸优化问题（MICP）

■ 3.1 凸集图中的最短路径问题（SPP）

最短路径问题（SPP）在凸集图（GCS）中，是经典最短路径问题的推广。给定一个带有有向图 的问题，其中 是顶点集， 是边集。每个顶点 关联一个有界凸集 。

不同于经典的最短路径问题，该问题中每条边的长度是由顶点之间的连续变量 和 决定的，通过一个凸函数 来表示，并且该函数要求取非负值。

凸集图中的最短路径问题可以形式化为以下优化问题：

其中， 是图中连接起点顶点 和终点顶点 的有效路径， 是图中所有 到 的路径集， 是路径 所经过的边集， 是顶点 的变量。

■ 3.2 凸松弛的随机舍入

作者提出了一种通过凸松弛方法来解决最短路径问题，然后使用随机舍入策略来近似求解的框架。首先，通过求解凸松弛问题得到边的概率分布，然后通过深度优先搜索（DFS）和回溯算法来从这些概率中生成一条有效路径。

对于任意边 ，二元变量 被松弛为连续区间 。在每条边 上，其值 可以被自然地解释为该边成为最短路径一部分的概率。在随机舍入过程中，给定当前的顶点 ，算法以概率 选择下一条边 ，并继续构建路径，直到到达目标顶点 。

最终，该框架提供了一个自动生成最优解近似的能力，即通过比较凸松弛解和舍入解的成本，估算最优性能误差：

且可被估算为：

这种方法在很多实际问题中非常有效，并且在大多数情况下，简单的舍入操作就足以生成全局最优的无碰撞运动轨迹。此外，整个过程的计算成本仅相当于一次凸优化，极大地降低了传统混合整数规划的计算开销。

在上一部分，作者构建了整体的优化框架，但如何从第二部分描述的问题构建优化框架中所要求的GCS，是这一章所研究的重点。

这一部分的核心是将无碰撞运动规划问题转化为凸集图（GCS）中的最短路径问题，并通过凸优化方法来求解轨迹和时间缩放函数。具体而言，目标是让机器人在一系列凸区域中移动，并通过连接这些凸区域的路径来生成无碰撞的连续轨迹。

■ 4.1 图的构建

首先，作者通过将机器人所在的配置空间分解成若干个“安全”凸区域 ，然后通过图 来表示这些区域的连接关系。每个顶点 对应一个安全的凸区域 ，并且两个凸区域之间的交集决定了是否在图上添加一条边。额外地，起点顶点 和终点顶点 被作为辅助顶点加入图中，用于保证初始和最终状态的边界条件。

边集 的定义如下：

这表明，如果两个区域 和 存在交集，则它们之间存在一条边。此外，如果起点 或终点 位于某个区域 内，则起点和终点也会与该区域相连。

■ 4.2 凸集的定义

接下来，为每个顶点 定义一个凸集 ，其中包含对应贝塞尔曲线的控制点 和时间缩放函数 的控制点 。这些控制点定义了在该区域内的轨迹片段和时间缩放曲线。凸集的定义如下：

这些条件确保了贝塞尔曲线的所有控制点 位于对应的凸区域内，同时保证时间缩放函数 单调递增，并对机器人速度施加硬性约束。

■ 4.3 边上的约束

在每条边 上，作者使用了线性约束来确保不同区域之间的轨迹是连续可微的。对于每条边 ，定义以下约束：

这些约束确保了轨迹和时间缩放函数在通过边界时的连续性，使得全局轨迹具有 阶的连续性。

■ 4.4 边的长度

为了将路径规划问题形式化为最短路径问题，边的长度 定义为轨迹的代价，包括时间、路径长度和能量消耗。

■ 4.5 重建无碰撞轨迹

一旦最短路径问题（SPP）求解完成，最优路径决定了机器人必须经过的安全区域序列。通过对这些区域中的贝塞尔曲线和时间缩放函数进行拼接，可以重建全局无碰撞轨迹 和时间缩放函数