穿越凸障碍物的运动规划优化（GCS）

从在城市地区运送包裹的四旋翼飞行器到在封闭仓库中移动的机器人手臂，围绕障碍物的运动规划是现代机器人技术的核心挑战。基于优化的规划者可以在满足机器人动力学要求的前提下，在高维空间中设计轨迹。然而，在存在障碍的情况下，这些优化问题变得非凸且很难求解，甚至只是局部求解。因此，当面对混乱的环境时，机器人专家通常会退回到基于采样的规划者，这种规划者不能很好地适应高维，并与连续的微分约束作斗争。在这里，我们提出了一个框架，使凸优化，以有效和可靠地规划轨迹围绕障碍。具体来说，我们关注的是无碰撞运动规划的成本和约束的形状，持续时间和轨迹的速度。利用最近在凸集图(GCS)中寻找最短路径的技术，我们设计了一个实用的凸松弛规划问题。我们表明，这种松弛通常是非常紧的，以至于一个廉价的后处理其解决方案几乎总是足以确定全局最优的无碰撞轨迹(在参数化的曲线类内)。通过数值和硬件实验，我们证明了GCS算法能够在更短的时间内找到更好的轨迹，并且能够在高维复杂空间中可靠地设计轨迹环境。
一句话总结:凸优化可以可靠地设计出机器人在复杂障碍物环境中运动的最优轨迹。

引言

高效的障碍物轨迹设计是机器人自主设计的一个重要挑战。从四旋翼机到机器人手臂，从自动驾驶汽车到有腿的机器人，几乎每一个现代机器人系统都依赖于防撞规划器来在其环境中移动。人们提出了各种各样的技术来解决机器人技术中这个长期存在的问题(1)，每种技术都有其各自的特点优势和局限性。数值优化为运动学和动力学约束下的高维空间运动规划提供了成熟的工具(2-7)。然而，通过将规划问题转化为非凸优化问题，并依赖于局部求解器，如果环境中存在障碍，这些技术往往无法找到可行的轨迹。尽管已经提出了多种方法来缓解这个问题(8-10)，但机器人专家在面对混乱的环境时通常更喜欢基于采样的规划者(11-13)。基于采样的算法在概率上是完备的，这意味着，如果存在一条可行的路径，它们最终会找到一条，不管有多少障碍。然而，即使使用渐近最优的基于抽样的规划者(14-16)，我们设计的轨迹在其中可能是相当不理想的高维，密集抽样是不可行的。此外，尽管这些算法中有许多支持运动动力学约束(17-19)，但连续微分约束很难强加于离散样本，这使得这些运动动力学变量在实践中不太成功。基于混合整数优化(20-24)的规划者的承诺是利用以上两个方面的优点:基于采样的算法的完整性，以及轨迹优化处理机器人运动学和动力学的方便性，以及全局最优性的额外好处。然而，这些技术的传播受到了它们的运行时间的严重限制，即使是小问题也可能只有几分钟的时间。凸优化(25)通常被认为是一种不适合设计绕障碍物轨迹的工具。一方面，考虑到精确无碰撞运动规划的计算困难，这种信念是有充分依据的(26,27)。另一方面，这些负面结果并不排除基于凸优化的近似运动规划者的可能性，它们在实践中通常遇到的问题上表现得非常好。在(28)中，规划问题的一个层次凸松弛被截断为低阶，代价是失去了规划算法的完整性。然而，这种特殊的方法仅限于多边形轨迹，而且只应用于低维度。提出了一种基于凸优化的近似算法一类有限但重要的无碰撞规划问题的解(见电影1)。我们将这种规划图命名为凸集图(GCS)，以其底层优化框架(29)命名。GCS可以处理各种有用的差异成本和约束。尽管只依赖于凸规划，它设计的无碰撞轨迹几乎总是全局最优的选择类曲线。此外，GCS可以有效地扩展到高维，并且在问题数据的某些假设下，保证在规划问题允许解决方案时确定可行的轨迹。

结果

在这一节的开始，我们简要描述了GCS所解决的运动规划问题，并总结了我们算法的主要特性和保证。然后我们在各种机器人上演示GCS。首先，为了说明我们的方法的一些关键优势，我们考虑一个复杂迷宫中的规划问题。其次，我们证明了GCS能够可靠地合成动态可行且全局最优的轨迹(在参数化曲线类内)，用于四旋翼飞行穿过建筑物(见图1)。我们证明，与高度优化的基于抽样的规划方法相比，GCS可以在更短的时间内设计出更短的轨迹。最后，我们用14维的双手操作问题来说明GCS在实际硬件上的可扩展性(见图2和电影1)。

GCS轨迹优化

无碰撞运动规划的主要挑战是搜索空间的非凸性。与(23)类似，GCS通过对非凸避障约束的补充描述缓解了这一问题，其中机器人需要通过安全凸区域集合Q移动1……qn ⊂rd 不与障碍物相交。这将导致一个离散组件(选择安全区域进行遍历)和一个连续组件(在每个区域内设计机器人轨迹)的优化问题。GCS利用一种高效的融合了凸优化方法的图搜索和轨迹优化方法来解决这个问题。以下是GCS处理的规划问题的一个基本例子:

q是要设计的轨迹，T是它的持续时间，它本身就是一个决策变量。成本是轨迹弧长L(q) =的加权和，用户指定的权重a, b≥0 R0 T˙2dt和持续时间t第一个约束通过要求机器人构型在任何时候都处于一个安全集合来确保避免碰撞。注意，这是一个比基于采样的运动规划更强的约束，在采样的运动规划中，轨迹通常在有限数量的点上被检查为无碰撞。第二个约束条件使速度q(t)˙在所有时刻都处于一个凸集合D内。其余约束条件对机器人构型及其导数施加初始和最终条件。
GCS可以处理的额外成本和限制包括对机器人速度的凸惩罚，以及对轨迹持续时间和长度的硬性限制。我们也可以要求轨迹任意可微很多次:这使得我们可以为全驱动和差动平系统设计动态可行的轨迹，比如机器人手臂和四旋翼。最后，为了使轨迹更加平滑，在下面的实验中，我们还将考虑对机器人加速度的惩罚，尽管这些目前没有像我们的规划者所做的速度成本那样有效地处理(参见材料和方法)。
我们的算法将规划问题(1)简化为GCS(29)中的最短路径问题(SPP)。问题的离散部分是由一个编码安全集合之间连接性的图来建模的。然后使用来自(29)的技术来激活和取消依赖于我们在图中采用的路径的连续机器人轨迹上的成本和约束。(见图3表示GCS中的SPP，图4表示将规划问题(1)简化为GCS中的SPP。)最终的优化问题是天生的混合整数，但是，与常见的混合整数公式(23,24)相比，它的凸松弛通常是非常紧的。在这一点上，GCS的操作只需要两个步骤:它解决了混合整数程序的凸松弛，并通过廉价的后处理恢复了无碰撞轨迹。第一种是典型的线性规划(LP)或二阶锥规划(SOCP)，它们可以用常用的求解器有效地求解。第二个是一个随机四舍五入算法，它产生一个整数可行解，没有任何分支和界。在下面的实验中，这些圆形解几乎总是全局最优的原始混合整数问题。

算法属性和保证

GCS做了一些近似来有效地解决规划问题(1)。这里我们简要描述这些如何影响我们的轨迹的可行性和最优性。
第一个近似是安全配置空间Q⊂R的描述d 作为凸区域Q的并集1……qn。虽然这种描述可以精确地描述多边形障碍，但一般来说，自由空间的近似分解是必要的。对于这一步，有多种实用算法(30-33)，以及针对运动树复杂构型空间的方法(34-37)。第二个近似是轨迹参数化，这是数值解决我们的问题所必需的。这里我们使用B´ezier曲线:这些曲线对运动规划具有许多有用的特性(38-42)，它们允许GCS设计轨迹，保证在任何时候都是安全的，没有离散化错误。这些近似的影响可以通过增加安全区域的数量和B´ezier曲线的程度来减少，而只会轻微地(多项式地)影响GCS的运行时间(见材料和方法)。

直到空间分解和轨迹参数化，我们的混合整数规划问题是精确的。本文的分析重点是评估该混合整数规划的凸松弛的紧密性和通过舍入得到的轨迹的质量。为此，GCS自动为我们提供了它所设计的轨迹成本和混合整数规划的最优值之间的严格界限。让Crelax ≤copt ≤cround 分别是松弛、混合整数规划和四舍五入解的(非负)代价。最优间隙δopt 我们的轨迹可以被高估如下，不需要额外的计算:

注意间隙δopt 以及δrelax 取决于具体的问题实例。为了简单起见，我们说一个轨迹q，使δopt = 0为最优;没有指定它仅在曲线的参数化类中。
在某些应用中，规划算法的健全性和完备性比最优性更重要。GCS是一种良好的运动规划器，这意味着它找到的轨迹在任何时候都满足问题(1)的所有约束条件。在对弹道持续时间T没有严格限制的情况下，直到空间分解的保守性，GCS也是完整的:如果存在可行的弹道，它保证识别出可行的，否则证明不可行(见材料和方法)。

迷宫中的运动规划

对于大多数运动规划者来说，在迷宫中设计一个平滑的轨迹是一个非常具有挑战性的问题。以带有50的迷宫为例2 = 2500个细胞，如图5所示。通过对初始轨迹的微小扰动，局部优化几乎没有机会找到穿越这个迷宫的方法。现有的混合整数规划者也会失败，因为他们将轨迹参数化为一系列曲线，并使用二进制变量将每条曲线分配给每个单元格(23)。鉴于此一个轨迹可能需要访问每个细胞，这将需要2500个细胞2 ≈107 二元变量:一个超出任何求解器能力的量。在图5中，基于采样的方法可以很容易地发现穿过迷宫的路径，但它们会遇到持续的差异成本和约束。另一方面，GCS可以有效地设计平滑的轨迹，同时显式地利用这个问题下的图结构。

为了将图5中的问题以GCS要求的形式表达出来，我们将每个迷宫单元设为一个安全集Qi。(在本文中，i和j被理解为{1，…， n})我们总共有n = 2500个安全集，每个安全集是一个单位平方。初始q0 最后一个qT 点是迷宫的入口(左下)和出口(右上)。我们考虑两个问题:首先我们寻找穿过迷宫的最短连续轨迹(问题(1)中的代价权重为a = 1和b = 0)，然后最小化轨迹持续时间(a = 0和b = 1)，并对加速度进行较小的惩罚。在第二个问题中，我们还要求轨迹可微两次，我们强制速度限制D =[−1,1]2 边界条件q˙0 ˙T = 0。
GCS设计的轨迹如图5所示，红色虚线表示第一个问题，蓝色实线表示第二个问题。在这两种情况下，凸松弛和圆角解的代价相等，因此GCS自动证明这些轨迹是最优的(δopt δrelax = 0)。当目标是最小化轨迹长度时，我们的凸松弛是一个soc(见材料和方法)，在0.98秒内解决。最小时间问题是一个LP问题，但由于高度轨迹参数化，它实质上比soc大，需要9.1秒才能求解。我们强调，这些是具有大约5,000个二进制变量而不是10个二进制变量的混合整数程序的松弛7 (23)所要求的二进制文件。
这个例子突出了GCS混合离散和连续优化的透明度。在迷宫中找到离散的细胞序列是一种简单的图形搜索，温和的差异成本和约束不应该使问题变得更加困难，特别是如果我们寻求近似的解决方案。虽然现有的算法无法融合离散和连续的运动规划特性，但GCS可以有效地设计平滑的轨迹，同时明确地利用我们问题背后的图结构。

统计分析:四旋翼飞越建筑物

我们使用GCS来计划四旋翼机通过随机生成的建筑的飞行。这种任务的一个例子如图1所示:当从起始平台移动到结束平台时，四旋翼需要绕着树飞行，并穿过门窗。对于每个建筑，房间的数量、墙壁的形状、门、窗和树的位置都是随机选择的。起点总是在建筑物外面，而目标总是在里面。
尽管四旋翼飞行器的构型空间是六维的，但该系统的微分平坦性允许我们直接在三维笛卡尔空间中规划轨迹。事实上，给定质心可微四次的任何轨迹，四旋翼方向的动态可行轨迹，以及必要的控制推力，总是可以被重建(43)。
假设所有障碍物都是多边形的，将自由空间分解为凸集Qi 完全正确。将四旋翼飞行器的碰撞几何形状视为球体。代价是惩罚轨迹长度和持续时间(a = b = 1)，包括一个小的加速度惩罚。为了利用微分平面性，我们要求轨迹可微四次。速度有边界条件q˙0 ˙T = 0，并在方框中受限

我们通过100个随机建筑来规划四旋翼的运动，并分析最优性差距δopt 以及δrelax。δ值opt 为了分析目的，是通过求解来计算的吗
利用分支和界求解全局最优性的混合整数问题。间隙δrelax ≥δopt 由GCS自动返回。这些值在整个实验中的直方图见图6。在95%的环境中，GCS设计的轨迹具有最优间隙δopt ≤1%，而且，即使在最坏的情况下，我们也只有δopt = 2.9%。对于68%(分别为84%)的问题，GCS证明设计的轨迹在δ范围内relax = 4%(分别δrelax = 7%)的最佳。GCS认证的最大最优间隙为δrelax = 27.1%，对应于存在δ的环境opt = 2.3%。因此，即使在这种情况下，δ值也比较大relax 主要是由于凸松弛稍微松散，而不是轨迹是次优的。这些问题的缓和是socp，而GCS的平均运行时间是2.65秒(包括舍入)。

与基于抽样的规划器的比较:密闭空间中的机械臂

GCS是一种多查询算法，因为相同的数据结构(我们的优化问题背后的图)可以用于在许多初始和最终条件之间规划运动。它的自然的基于采样的竞争对手是概率路线图算法(11)。本文对d = 7自由度机器人手臂(KUKA LBR iiwa)的GCS方法和PRM方法进行了比较。我们选择这种机器人是因为PRM可以在更大的空间中挣扎，而GCS和PRM都可以在更低的维数下轻松地设计轨迹。
机器人环境如图7所示，由多个货架和两个箱体组成。自由空间的精确分解在这里是不可行的，因此我们采用(36)中的近似算法。给定机器人的“种子构型”，该算法膨胀一个不与障碍物碰撞的机器人构型多面体。区域的自动播种是可能的，但我们发现通过逆向运动学手工生产种子，以及区域Q的简单可视化连接i，简单而高效。我们构建了n = 8个安全多面体i 使用这个工作流:五个种子为了覆盖夹持器接近架子和箱子的配置，三个被播种来填充剩余的空闲空间。安全区域的构建是并行的，耗时50秒。

在实践中，PRM生成的轨迹可能是次优的，很少直接发送给机器人。即使PRM存在渐近最优版本(14)，根据我们的经验，在这里考虑的相对高维空间中，这些变体性能的提高并不值得它们的计算成本。一个常见的解决方法是使用一个简单的快捷算法对PRM轨迹进行后处理。这是沿着轨迹对点的采样，并通过直线段将它们连接起来:如果一个段是无碰撞的，那么轨迹就成功缩短了。这一步可以大大缩短PRM轨迹，但它需要许多碰撞检查:因此，我们将GCS与常规PRM和有捷径的PRM进行比较。我们使用基于(44)的高性能PRM实现，并很快被包括在开源软件Drake(45)中。这个比较中的PRM有10,000个节点。它的构造需要0.54秒，在补充材料的S1节中有描述。请注意，这一次是符合最先进的PRM实施(46-48)。
这个比较中的任务如图7所示，要求手臂在五种配置之间移动1……ρ5 ∈Q，同时避开货架和箱子。构型ρ1ρ2，和ρ3 对应于夹爪在架子上面，在第一架子，在第二架子。路径点ρ4 ρ5 对应左边和右边的bin。当k = 1时，…4，任务k要求机器人从ρ移动k ρk+1。任务5需要从ρ开始5 回到ρ1。目标是用最小长度的连续轨迹(a = 1和b = 0)连接起点和目标构型。
图7所示为每个规划器和每个任务的机器人抓手轨迹。

蓝色表示GCS，黄色表示PRM，红色表示PRM有抄近路。这些轨迹的长度，以及每个规划器的离线和在线运行时间，用匹配的颜色在图8中报告。(虽然GCS的舍入阶段是随机化的，但图8只显示了它的轨迹长度的一个值，因为重复多次实验GCS总是会找到相同的解。)在所有的任务中，GCS设计了更短的轨迹，并且在网络上，它比两个PRM竞争对手跑得一样快，甚至快得多。
对于这些问题，我们的凸松弛是一个SOCP，由于安全区域的个数很低，n = 8，以及轨迹的一个便宜的多边形参数化，所以求解速度非常快(见材料和方法)。在空间分解的保守性范围内，GCS设计的所有轨迹都是最优的(δopt = 0)。经认证的最佳间隙δrelax 平均为4.1%，并且在第一个任务中达到了13.0%的最大值。
虽然仅限于五个任务，但这种比较显示了一个普遍趋势。GCS的离线计算比PRM的要求更高。然而，这种额外的努力会在在线阶段得到回报，GCS可以在更短的时间内找到更好的轨迹和更强的避碰保证。对于那些环境实际上是静态的应用程序来说，这是非常有价值的，改善在线性能值得投入更多离线预处理中的工作量。

两个机器人手臂的协调规划

在之前的比较中，我们选择了一个d = 7关节的机器人，因为PRM方法在更高的维度上表现不佳。为了演示GCS的可扩展性，我们设计了一个d = 14自由度的双臂机械臂的运动，如图2所示。除了避免与环境的碰撞，现在的GCS还必须防止自我碰撞之间手臂。
我们考虑图2和电影1中所示的三个任务。在第一个任务中，机器人从前面的架子上抓取两个杯子，并将它们放回需要的位置双臂交叉。在第二个任务中，它移动两种喷漆并达到非常接近自碰撞的配置。在第三个任务中，一只手把一个小盒子递给另一只手。每个任务都是作为一个单独的凸程序来解决的:当手臂抓住、放置或离开一个对象时，它的配置是预先指定的，但对象的操作顺序是优化的。例如，在第一个任务中，GCS可以决定先抓哪个杯子，或者是否抓同时进行。
使用(36)中的算法再次分解空闲空间。对于第一个环境，我们构建了35个多面体，在4种可能的碰撞几何形状下覆盖工作空间(架子上的杯子，左手拿杯子，右手拿杯子，双手拿杯子)。我们同样进行第二和第三个任务，其中包含12和14个安全的多面体。因为这里我们不比较PRM，我们不限于多边形曲线，我们规划的二次可微轨迹在长度和持续时间上具有相等的惩罚a = b = 1，并且有一个小的加速代价。机器人的速度限制是通过一个盒形约束集D来实现的。
如图2所示，GCS设计了无碰撞轨迹，有效地完成了这三个任务中的每一个。最大最优间隙δrelax 是在第二个任务中，只有13.9%。在运行混合整数求解器后，我们验证了每个轨迹的实际最优差距不大于δopt = 8.4%。由于这些问题的凸性松弛是非常大的socp，它推动了GCS当前能力的极限，这些任务的运行时间为185、103和21秒。然而，正如下面所讨论的，我们相信这些时间在不久的将来会大幅度减少。

讨论

结果表明，GCS能够处理连续的差异成本和约束，利用了规划问题的组合结构，并且具有较低的运行时间。虽然存在美国的规划师只具备其中的一两个特点，而GCS则囊括了这三个特点。另一方面，我们的方法有重要的先决条件，如所有问题组件的凸性和自由空间的分解。在本节中，我们将扩展GCS的优势和局限性。

与现有算法的定性比较

GCS具有现有混合整数规划的主要优势，即混合离散和连续优化的能力。事实上，GCS本质上是一个混合整数规划器，但是，通过图建模问题的离散部分，并利用(29)的技术，它产生的混合整数优化问题从根本上不同于之前提出的问题。与现有的方法相比，我们的公式的凸松弛方法更紧凑，计算上也更轻。现有的方法在二维空间中通过十几个凸区域生成轨迹需要数十秒(23)。
如上所述，PRM是基于采样的GCS的天然竞争对手。事实上，GCS可以被认为是PRM的一般化，其中每个无碰撞样本被扩展到一个无碰撞凸区域，该区域在障碍物允许的范围内膨胀。GCS不是密集地采样配置空间，而是用几个大的凸集填充配置空间，将组合复杂度降低到最小，并利用有效的凸优化来通过开放空间进行规划。我们已经看到，在在线运行时间和轨迹成本方面，GCS优于PRM，并且它可以扩展到更高的维度，处理 更多种目标函数和约束条件。此外，GCS的轨迹保证在任何时候都是无碰撞的，而不仅仅是在有限数量的点上。
基于采样的方法的一个优点是，它们通过采样迭代地探索自由空间，而GCS将部分规划问题的计算复杂性转移到自由空间的离线凸分解上。在一般情况下，这个分解步骤是困难的，即使我们对大致的覆盖范围感到满意(49)。然而，正如结果所示，对于许多现实世界的规划问题，几个大的区域就足以在比现有运动规划者更少的时间内设计出更好的轨迹。此外，通过最近的区域膨胀技术(33 - 36)和下一小节讨论的实用权宜之计，这些区域的构造相对简单。我们还强调，基于抽样的方法由于其简单性，在学术界和工业界被广泛使用。GCS的实现并不简单，但我们已经提供了本文和(29)中描述的技术的成熟实现，在开源软件Drake(45)中。
基于局部非凸优化的规划者可以处理几乎所有的成本和约束，包括动态约束和任务空间约束，但它们可能缓慢且不可靠。GCS在精神上是不同的，因为我们优先考虑低运行时间和算法保证高于建模能力。在许多应用中，用局部优化器对轨迹进行后处理也是可行的，将GCS在复杂空间中寻找轨迹的能力与局部优化的通用性结合起来。

自由空间分解

区域膨胀技术(33-36)为我们提供了一种直观的方法来构造覆盖复杂构型空间大部分的安全凸集。一般来说，量化近似空间分解对轨迹成本的影响是困难的。然而，当我们使用更多的区域来覆盖自由空间时，这些效应就消失了，并且可以很容易地使用(50)中的伯努利试验来估计覆盖空间的百分比。此外，如迷宫问题和材料和方法所示，GCS的运行时间随着区域数n的增加而略有增加。与自由空间覆盖一起，我们在GCS中的SPP下面的图是另一个有用的指标，表明凸分解对于规划有多合适，因为它显示了自由空间的哪些部分可以通过轨迹连接，并指出在哪里去播种新的区域，改善当前的分解。
对于杂乱的环境，手工播种安全区域是不切实际的。在这些情况下，将GCS解释为PRM的泛化，使我们能够从基于抽样的方法中利用许多成功的思想。目前，我们正在开发一种自动化的区域膨胀算法，用于高度杂乱的环境，它建立在基于可见性的PRM之上(51)。同样，我们希望为PRM开发的处理动态环境的技术(52,53)可以以相对较低的努力扩展到GCS。

未来方向

尽管GCS已经可以与广泛使用的运动规划器相媲美，并且正在寻找多个真实世界的应用，但它还有许多可以发展的方向。还有空间让GCS变得更快:我们目前正在为我们的凸程序开发一个定制的GPU求解器。我们还将扩大支持的成本和限制。最后，我们想要发展任务空间和输入限制约束的紧凸近似。我们希望我们的规划者能够促进凸空间分解的高性能算法的发展，并且它展示了在GCS中作为spp来表述机器人中的许多其他问题的潜力。例如，GCS中的SPP可能是正确的“建模语言”，用于涉及机器人与环境之间接触的控制问题，以及综合任务和运动规划(54)。

材料与方法

GCS将规划问题(1)简化为GCS(29)中的一个SPP，求解该SPP的紧凸松弛，并使用简单的舍入算法恢复一个近似解。在这一节中，我们首先报告必要的背景在GCS中的SPP和它的放松。然后对所提出的舍入策略进行了说明。最后，我们描述了轨迹参数化利用B´ezier曲线，将GCS规划问题简化为SPP。
###凸集图中的最短路径
图3提供了GCS中SPP的视觉描述。在这个问题中，我们已知一个顶点V和边E的有向图G = (V, E)，每个顶点V∈V都与一个凸集X配对v 点xv ∈xv。边e = (u, v)∈e的长度由x的连续值决定u xv 通过表达式’e(xu， xv)。函数’e 要求是非负凸的。此外，形式(xu， xv∈xe 可以用来耦合一条边的端点。对于固定源顶点σ和目标顶点τ，路径p是σ通过子集E连接到τ的不同顶点序列p 用P表示图G中所有路径的集合，GCS中的SPP表示为
尽量减少受

变量是离散路径p和连续值xv。代价是路径p的长度最小，定义为它的边长之和。第一个约束条件要求p是σ到τ的有效路径。重要的是，剩下的约束只适用于与路径p上的顶点配对的连续变量。

凸松弛的舍入

虽然GCS中的SPP在计算上比较困难，但它可以被表示为一个紧凸松弛的紧凑混合整数规划(29)。我们的策略是解决这个松弛和检索整数可行解使用一个廉价的四舍五入算法，没有任何分支和界。我们将在本节的最后看到，没有对轨迹持续时间的硬性限制T，对于足够程度的轨迹参数化，保证该方法能识别出可行解。
from(29)的混合整数程序用二进制变量y参数化路径pe ∈{0,1}每个边e∈e，与ye = 1当且仅当e∈ep。在凸松弛法中，二元约束被松弛到ye ∈[0,1]，y的最优值e 可以很自然地解释为边e沿着最短路径的概率。对于舍入步骤，我们提出了一个随机深度优先搜索。从源σ开始，在每次迭代中，该搜索以y的概率穿过一条边ee，由当前顶点出的边的概率和归一化。如果出现了一个死胡同(所有非零概率的出边都指向我们已经访问过的顶点)，我们就会往回走。当到达目标τ并找到路径p时，算法终止。选择这条路径可以将GCS中的SPP简化为带带状约束的小凸程序，可以快速求解出路径开销和连续变量x的值v。
为了增加找到低成本路径的机会，我们多次应用四舍五入。四舍五入试验是可平行的，相对于松弛而言，它们所花的时间可以忽略不计。对于上面的数值结果，我们进行了十次四舍五入:根据我们的经验，这对于大多数问题来说已经足够了。自动决定需要多少次试验的一个简单启发式方法是，为不能识别新路径的连续循环设置一个限制。为了便于说明，图S1显示了与PRM比较的5个任务的边缘概率，以及通过随机四舍五入找到的相应路径。

B´ezier曲线

为了用有限数量的变量来参数化轨迹，我们使用B´ezier曲线。对于B´ezier曲线的定义，我们将读者指向(55);在本文的范围内，我们可以说B´ezier曲线是一个多项式函数γ:[0,1]→Rd 是线性的参数化的m + 1控制点γ0……， γm ∈rd。
B´ezier曲线具有许多有用的性质。终点性质告诉我们曲线γ的起始点γ(0)是第一个控制点γ0，末点γ(1)为末点γm。凸包特性保证了曲线γ完全包含在其控制点的凸包中。最后，曲线γ的导数γ˙也是B´ezier曲线，它的度为m - 1，它的m个控制点是γ的控制点的线性组合。

基于GCS的障碍物运动规划

现在我们将展示如何将问题(1)简化为GCS中的SPP。为了在GCS中形成一个SPP，我们需要:定义一个图G，分配一个集合Xv 到每个顶点v，并将每条边e与约束集X配对e 以及一个长度函数’e。下面我们将描述这些组件是如何构造的(参见图4中的插图)。在高层次上，我们的计划是构造一个图G，它的路径p表示安全区域Q的不同序列i 机器人可以穿越到达目标。每个安全区域然后与轨迹段q配对i，使用(29)中的框架对具有足够代价的路径p的选择和轨迹段q的关节形状的连续性约束进行耦合i。

GCS中SPP下的图G = (V, E)是安全区域Q的交图1……qn，如图4(A、B、C)所示。具体来说，每个集合Qi 与顶点i∈V配对，并由边(i, j)∈E连接到所有区域Qj 和它相交。初始配置q0 用源顶点σ表示，用边(σ， i)连接到每个区域Qi 这包含了它。类似地，qT 与目标顶点σ配对，并通过边(i， τ)连接到包含它的每个区域。那么我们G中的路径p代表连接起点和目标的安全区域序列构型。
顶点上的集合源σ和目标τ是用于执行轨迹边界条件的辅助顶点:它们不需要决策变量，并且它们与空集合X配对σ = xτ ∅。另一个凸集Xi 有一个更复杂的角色，因为它们限制了参数化我们在安全区域Qi。
我们分配一个轨迹段qi : r→rd 每个安全区域Qi，见图4(D)。这些线段使用两条m度的B´ezier曲线参数化:一条曲线ri :[0,1]→rd 它参数化了轨道q的形状i，和曲线hi :[0,1]→R表示气运动的速度。在数学上，qi是ri◦h−1i的复合函数。对于每个顶点i，我们配对一个凸集Xi 哪一个元素是两条曲线r的控制点i hi，即xi = ®i,0， hi,0……， ri,m， hi,m)。集合Xi 需要保证:曲线ri (因此qi)完全包含在凸集Q中i时标度函数hi 是严格递增的(因此是可逆的)，速度q˙i(t)时刻都在凸集D内。对于第一个问题，B´ezier曲线的凸壳性质使它足以简单地要求ri,k ∈qi 当k = 0时，…第二个也类似:因为导数h˙ i 是B´ezier曲线，它的正电荷是由它的控制点h˙ i,k 当k = 0时，…， m−1。最后，利用类似的逻辑，速度约束可以被条件r˙所暗示i,k/h˙ i,k ∈D对于k = 0，…M - 1(由于h˙，M - 1是凸的 i,k > 0)。

边缘约束的第一个作用是施加边界条件。特别地，对于从源出发的所有边e = (σ， i)，我们想要得到ri(0) = q0， hi(0) = 0，而r˙i(0)/h˙ i(0) = q˙0。这是通过定义X实现的e 通过等式ri,0 = q0， hi,0 = 0，而r˙i,0 ˙ i,0˙0，它们在决策变量x中是线性的i。类似地，对于所有边e = (i， τ)我们想要的是ri(1) = qT r˙i(1)/h˙ i(1) = q˙T 。因此，我们通过ri,m = qT和r˙i,m - 1 = h˙i,m - 1q˙T定义Xe。其次，边缘约束必须确保轨迹q的串联i qj 如果我们沿着边(i, j)过渡，它是充分可微的i qj 通过让Xe， e = (i, j)，执行ri,m = rj,0 hi,m = hj,0。而对于一阶导数，我们要求r˙i,m - 1 = r˙j,0和h˙i,m - 1 = h˙j,0，高阶导数也是如此。

边的长度e 必须通过适当加权图g中每个转移的代价来重现我们的计划问题中的代价。这是通过将长度为0的边(σ， i)赋值给从源发出的边(i, j)和(i， τ)，以及长度aL(ri+ b(hi(1)−hi(0))。第二项是决策变量b(h)的线性函数i,d −hi,0)。在我们的决策变量中，第一项是凸的，但它不允许一个封闭的表达式。然后我们惩罚计算上便宜的上界

这高估了r的长度i 通过将控制点之间的距离相加。
一旦求解了GCS中的SPP，最优路径p就确定了区域Qi 我们的机器人必须穿越。为了重建轨迹q，我们将与这些区域相关联的轨迹段qi = ri◦hi−1排序，如图4(E,F)所示。数学上，q(t) = qi(t)对于所有t∈[hi(0)， hi(1)]，对所有I∈p{σ， τ}。

正如结果中强调的那样，上面的构造涉及一些近似。凸分解的影响可以通过增加安全区域的数量来减轻。通常情况下，我们的图G的大小随n线性增长，而且，根据经验，我们的松弛的紧度与它无关。因此，GCS的总体运行时间仅随该参数多项式增加。其次，我们有轨迹参数化。B´ezier曲线不能表示所有的轨迹，要求控制点位于一个凸集是这是包含整个曲线的一个充分条件。类似地，强制r的连接i 用r表示j hi 用hj 可微仅对q的串联是充分的i 用qj 要这样。通过增加曲线的m度(56)，这些间隙可以降低到任意精度。这增加了集合X的维数v 而GCS的运行时间只有多项式。在这些近似下，SPP的解对于问题(1)，GCS给出了一个最优轨迹q。

健全和完整

由于B´ezier曲线的性质，我们的轨迹满足规划问题中的所有约束条件，没有离散化误差。这使得GCS成为一个良好的运动规划器。完整性涉及更多，并且只能达到空间分解的保守性。如果没有从q开始的轨迹0 To qT 通过安全区域Qi，我们的图G不存在从σ到τ的路径，我们的凸松弛是不可行的。如果存在可行轨迹，松弛是可行的，深度优先搜索在g中至少识别出一条路径。如果轨迹持续时间T不受约束，我们可以很容易地计算出B´ezier曲线的m度的有限下界，以确保在舍入阶段求解的凸方案的可行性。这保证了仅通过使用凸优化，在达到空间分解精度的情况下，GCS识别出可行的轨迹(如果存在)，并证明不可行的轨迹。

附加说明

在结果中，我们使用了小的加速惩罚来平滑我们的轨迹。在《GCS》中加入精确的加速惩罚是很复杂的，因为加速问题i 是一个复杂的非线性函数的曲线ri hi，这是我们的优化变量。然而，有一种实用的方法可以防止qi 从太大到太大就是增加一个小的惩罚直接在r¨的量值上i 然后呢?i，并防止h˙˙ i 离零太近了。类似的策略可以用于正则化高阶导数。
(29)映射多面体集Xv 和D到线性约束的函数。类似地，线性边长度’e 产出线性成本。相反地，我们的B´ezier曲线的长度上界使用欧几里德范数，它导致了二阶锥约束的松弛。这就是为什么，对于多面体集合Qi 和D，我们的松弛是最小时间问题(a = 0)的LP和长度惩罚问题(a > 0)的soc。
在数值结果中，曲线ri hi 最小距离问题(迷宫问题与PRM比较)的度为m = 1，最小时间迷宫问题的度为m = 6，四旋翼飞行器的度为m = 7，双臂的度为m = 4。我们没有注意到使用更高度数的曲线会大幅降低成本。(注意，对于一个η可微的轨迹，我们需要m≥η + 1度的曲线。)

结果再现

对于一个成熟的GCS实现，我们将读者引向开源软件Drake(45)。复制本文中结果所需的代码可以在(57)和https:// github.com/RobotLocomotion/gcs-science-robotics上找到。所有的实验都是在一台拥有Threadripper 3990x处理器和256gb RAM的计算机上进行的。凸优化问题的求解器为MOSEK 10.0。

补充材料

补充方法:PRM的构建。
图S1: GCS和PRM比较的边缘概率。
23

尽管存在14个自由度、潜在的自碰撞和受限环境，我们的方法仍然可以可靠地规划出两个机器人手臂的关节运动。(A)手臂抓住架子上的两个杯子，并把它们倒放回去。(B)两种喷漆互换了。©一只手抓住一个糖盒子并把它递给另一只手。

33

GCS中的SPP是我们运动规划算法的优化框架。在这个问题中，我们有一个图，它的顶点与凸集合中约束的连续变量配对。一条边的长度是一个凸函数，它的变量与这条边连接的顶点配对。我们寻求连接源顶点σ和目标顶点τ的最小长度路径p，并允许沿着这条路径优化连续变量。例如，蓝色路径的长度p = (σ， u, v， τ)取决于为蓝色连续变量x选择的值σ， xu， xv， xτ ，而不是xw。

34

我们将运动规划问题简化为凸集图中的最短路径问题。(A)有两个障碍物的机器人环境，初始配置q0，和最终构型qT 。(B)自由空间分解为凸安全区域Qi。©相交图G用于空间分解，每个区域Q有一个顶点ii 顶点σ和τ表示初始和最终构型。(D)轨迹段qi 分配到每个区域Qi。(E,F)通过图中路径p，激活相应轨迹段关节形状的代价和约束。

35

红色虚线轨迹和蓝色实线轨迹是通过我们的方法生成的，分别是带有速度约束和加速度惩罚的最小长度目标和最小时间目标。对于这两个问题，我们的算法通过一个单一的凸优化问题来识别和证明一个最优轨迹。

36

我们的算法被用来计划四旋翼机通过100个随机生成的建筑的飞行。对于这些轨迹，两个直方图说明了实际的最优间隙δopt 最优间隙δrelax ≥δopt 由我们的方法自动认证。在95%(100%)的问题上，我们的算法设计了一个最优差距小于1%(3%)的轨迹。对于68%(84%)的问题，它还证明了轨迹的最优差距小于4%(7%)。

37

在5个任务上，我们的运动规划器是根据概率路线图(PRM)方法进行基准测试的。本文考虑了PRM的两种变体:标准PRM和遵循捷径算法的PRM。我们的方法用蓝色表示夹持器轨迹，黄色表示PRM，红色表示有捷径的PRM。

38

在5个任务上，我们的运动规划器(GCS)是根据概率路线图(PRM)方法进行基准测试的。本文考虑了PRM的两种变体:标准PRM和遵循捷径算法的PRM。图中显示了每个任务和每个规划器的轨迹长度和计算时间。与高度优化相比，GCS需要更昂贵的离线计算

PRM实现了，但在线设计的轨迹更短，时间更短。
39