Graphs of Convex Sets (GCS) 介绍

轨迹优化技术可以包含丰富的代价和约束范式，比如连续曲线上的导数约束，并可以扩展到高维构型空间，但是它们本质上是基于梯度优化的，所以会受到局部极小值的影响。而基于采样的规划方法全局地求解，并提供（概率）完整性的概念，这很难适应连续曲线上的导数约束，并且难以在高维度时求解。

为了解决这两个问题，MIT的Russ Tedrake等人提出了一种结合连续优化和组合优化的通用优化框架，称之为GCS[1]。首先思考一下，为什么PRM方法难以处理连续的曲率约束：在路线图构建阶段，进行了大量的碰撞检测工作，来证明连接两个顶点的直线段至少达到一定的采样分辨率下是无碰撞的。然后，online查询阶段，搜索离散图，找到从起点到终点的不连续的分段线性最短路径。但是为了满足之后的控制需要，一定要对该路径进行平滑，但是一旦平滑该路径后或者用连续曲线近似它后，就不能再保证该路径仍然没有碰撞，也不能保证它是最短的路径。因为offline路线图构建只考虑了线段，所以没有给后续online优化留下任何空间。当引入动力学时，这变得更加令人担忧——在动力学约束下，在运动学上可行的路径可能变得不再可行。

[1] shortest paths in graphs of convex sets

PRM工作流程的一个相对较小的变化是：每次选择一个样本，不仅仅是将构型空间中的点添加到图中，而是将该点扩展到构型空间中的凸区域。为了找到漂亮的大区域，付出一些offline计算的代价是值得的，因为这可以在online查询阶段带来巨大的回报。首先，可以制作更稀疏的图——少量的区域可以覆盖很大一部分构型空间，这也允许扩展到更高的维度。但更重要的是，现在有足够的灵活性可以online优化连续曲线，只要它们保持在凸区域内。这需要一个能够结合优化图上的离散路径和连续曲线参数的图搜索算法的泛化，这正是GCS可以提供的泛化。

（无碰撞）组态空间的凸分解

如果在构型空间中采样一个无碰撞的点，那么将该点膨胀成一个凸区域的正确方式是什么？在处理凸几何体时，答案变得更加简单。合理的做法是在欧几里得空间中用凸形状来近似机器人的几何形状。尽管许多机器人具有非凸的网格几何形状，通常可以将它们很好地近似为更简单的凸几何体的联合；有时这些几何体是圆柱体和球体等基本形状，或者可以直接对网格执行凸分解。但即使在欧几里得空间中几何形状是凸的，将它们在构型空间中视为是凸的是不合理的。有一些例外情况。如果构型空间只涉及平移，或者机器人几何形状旋转不变（例如，点机器人或球形机器人），那么构型空间仍将是凸的。

这对于移动机器人是可行的（甚至对于用球体近似的四轴飞行器）。在这种情况下，可以轻松计算采样点处任意两个凸体之间的最小欧几里得距离，例如，将点膨胀为适当半径的球体 [2]。为了找到更大的区域，可以使用了在 [3] 中引入并在Drake中实现的半定规划迭代区域膨胀算法（IRIS）。该算法在寻找分离超平面和寻找包含的椭圆的最大体积之间交替，以“膨胀”区域在局部最大化（高效近似）区域的体积。

[2] Sample-Based Planning with Volumes in Configuration Space
[3] Computing Large Convex Regions of Obstacle-Free Space through Semidefinite Programming

可以扩展IRIS算法以在更一般（非凸）的构型空间中找到大的凸区域。通过两种方式能做到这一点：
1、使用非线性优化：在IRIS-NP算法[4]（在Drake中实现）中，用非线性优化求解器（如SNOPT）代替了寻找最近碰撞的凸优化。随机采样潜在的碰撞，以使算法在概率上合理；在实践中，该算法相对较快，但不能保证该区域完全没有碰撞。
2、使用代数运动学+平方和优化：利用大多数机器人的运动学可以使用（有理）多项式来表达的想法，可以使用多项式优化的工具来搜索严格的非碰撞路径[5]（在Drake中实现）。这种方法往往比较慢，但结果是合理的。在这里必须做出的一个牺牲是，在原始空间的立体投影坐标中找到凸区域。在实践中，这表示坐标系（以及轨迹长度）在该区域上的轻微扭曲。

[4] Growing convex collision-free regions in configuration space using nonlinear programming
[5] Certified Polyhedral Decompositions of Collision-Free Configuration Space

如果继续遵循与PRM的类比，可以想象通过随机采样执行空间的凸分解，拒绝那些在碰撞或现有IRIS区域中的样本，然后膨胀任何剩余的样本。但事实上，可以做得比这更好——可以用相同数量的区域覆盖更大比例的配置自由空间，或者用更少的区域覆盖类似的体积。目前最好的执行这种凸分解的算法通过计算“可见图”[6]的“最小团覆盖”来实现（Drake版本实现）。

[6]Approximating Robot Configuration Spaces with few Convex Sets using Clique Covers of Visibility Graphs

在实践中，尝试计算7自由度机械手，甚至高达10自由度的构型空间的相当密集的覆盖似乎是可行的。但对于更高的维度，试图覆盖构型空间的每个角落可能是一个错误的目标。用代表构型空间“重要”区域的采样点初始化团覆盖算法的方法可能不错。有时可以通过手调（通过调用反向运动学）来做到这一点，或者，可以通过检查teleop demonstrations、来自另一个规划器（可以生成规划，但没有相同的保证）甚至来自强化学习策略的解决方案来实现这一点。

凸集图

GCS框架最初在[1]中介绍，在Drake中有一个成熟的实现。它是一个通用的优化框架，用于将组合优化（如图搜索）与连续优化（如上面研究的运动学轨迹优化）相结合。GCS可以为任何“网络流”优化提供连续扩展，但对于运动规划，感兴趣的第一个问题是图上的最短路径。在一个图中考虑从起点s找到（加权的）到达终点t的最短路径的非常经典的问题。该问题由一组顶点、一组（潜在定向）边和遍历每条边的代价来描述。
GCS为这个问题提供了一个简单但强大的概括：每当访问图中的一个顶点时，还可以从与该顶点相关的凸集中挑选一个元素。允许边的长度是相应集合中连续变量的凸函数，也可以在边上写凸约束（任何解路径都必须满足）。凸集图上的最短路径问题可以编码NP-hard问题，但这些问题可以表示为混合整数凸优化（MICP）。该框架之所以如此强大，是因为该MICP具有非常强且有效的凸松弛；这意味着，如果把二进制变量放松为连续变量，约等于得到了原始MICP的解。这意味着可以比以前的转录更快地将GCS问题解决到全局最优的数量级。但在实践中，仅求解凸松弛（加上一点舍入）几乎总是足以得到最优解。

收紧任何凸松弛的一个非常标准的方法是将约束相乘。通常，这会显著增加约束的数量，并增加问题的复杂性（例如，乘以线性约束会导致二次约束，这更难处理）。使GCS如此有效的关键观察结果如下：
1、由于图引入了自然稀疏性模式，只需要将少量约束相乘就可以看到巨大的改进。
2、使用透视函数机制[7,2.33]，可以在不（显著）增加优化复杂度类别的情况下，将二进制变量乘以凸成本和约束。

[7]Stephen Boyd and Lieven Vandenberghe, “Convex Optimization”

GCS公式具有许多良好的性能。例如，在所有集合都是点的情况下，凸松弛保证是紧的，从而将问题简化为离散图上的经典最短路径。[1] 有所有的细节。

GCS（运动学）轨迹优化

构型空间的凸分解，有（通用）GCS机制。剩下的就是将运动规划问题转化为GCS。“凸优化障碍物周围的运动规划”[8]一文中描述了转录。可以使用凸优化来有效地在固有非凸性的运动规划问题中得到全局最优解。

[8]motion planning around obstacles with convex optimization

IRIS区域提供了描述构型空间中可行点的凸集。但希望在构型空间中有可行的路径。但如果在同一IRIS区域中有两个点，那么知道连接它们的直线也保证是无碰撞的。当点位于两个IRIS区域的交叉点时，它们可以通过多个区域连接连续路径。这是转录的精髓。

这意味着，对于每次访问GCS中的顶点，都希望在IRIS区域中选择两个点，以便路径位于该区域中。因此，GCS中的凸集本身不是IRIS区域，它们是n维构型空间中具有2n个变量的集，其中第一个n和最后一个n都在IRIS区域中。在集合表示法中，可以说它是集合与自身的笛卡尔乘积。当IRIS区域相交时，在这些集合之间形成无向边，并且在边上施加约束，说一个集合的第二个点必须等于第二个集合中的第一个点。

使用Bezier曲线的凸包特性来保证固定次数的整个连续路径位于集合内，而不仅仅是在每个集合中放置一条线段。利用贝塞尔曲线的导数性质，可以在速度上添加凸约束（例如速度极限），并保证曲线即使在区域之间的交叉点也是平滑的。然而，为了定义这些速度，需要了解穿越路径的速率，因此还引入了时间缩放参数化，就像在上面的运动学轨迹优化中所做的那样，在每个凸集中都有时间缩放参数。

结果是一个相当丰富的轨迹优化框架，它编码了在GCS凸优化框架中想要的轨迹优化的许多（但不可否认的是，不是全部）代价和约束。例如，可以编写目标，其中包括：
1、时间（轨迹持续时间）
2、路径长度（用上限近似）
3、路径“能量”的上界
以及约束条件，包括：
1、路径导数连续性（任意程度）
2、速度约束（对所有t严格执行，而不仅仅是在采样点）
3、附加的凸位置和/或路径导数约束，例如初始和最终位置和速度。

自动施加路径连续性约束和区域约束（整个轨迹保持在IRIS区域的并集内），这些约束在所有t处都是有保证的，而不仅仅是在采样点。Drake实现。
当然，非凸轨迹优化消耗更丰富的代价和约束，但不具有GCS所能提供的全局优化和完整性元素。（当然，GCS有一些自然的非凸推广；首先关注凸优化版本，以确保深入理解这些转录。）不断在GCS轨迹优化框架中添加更多的成本和约束——一些看似非凸的约束实际上有一个精确的凸的重新表述，其他的有很好的凸近似。

变体和扩展

快速路径规划（FPP）[9]。许多运动规划实例不需要GCS的全部功能；启发式近似通常可以找到非常好的解决方案。FPP不是同时考虑离散和连续变量来求解完整的GCS问题，而是在求解离散和连续问题之间交替，以找到局部极小值。FPP（通常）比GcsTrajectoryOptimization更快，并且通过使用交替，它可以处理GcsTraditoryOptimization无法处理的导数约束（例如加速度约束）。然而，FPP更局限于它所能支持的凸集/约束的类别。（Drake即将实现）

[9]Fast Path Planning Through Large Collections of Safe Boxes

GCS在包括移动操作 [10] 和双臂操作 [11] 在内的流形上应用。

[10]Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets
[11]Constrained Bimanual Planning with Analytic Inverse Kinematics

具有动态约束的GCS。对于由分段仿射动力学和凸目标和约束描述的离散时间系统，GCS提供了迄今为止最强的公式[1]。也可以更直接地推断连续曲线上的连续动态约束（更多内容即将推出！）。

通过联系进行规划（即将推出！）。

不确定性下的规划（即将推出！）。

任务和动作规划（即将推出！）。[24]显示了从时间逻辑到GCS的自然转录。

GCS作为（反馈）政策（即将推出！）。

[12]Temporal Logic Motion Planning with Convex Optimization via Graphs of Convex Sets