《2013-Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization》

non-convex optimization problems

（一）Sequential convex optimization通过构造凸的子问题来解 非凸问题，主要有两部分

(1)一个使得约束步骤变小的方法  (2)一个将不可行约束转化为惩罚的策略

对于(1)，用了信任区域  对于(2)， 用到了 ` l1   penalties

用到了 ` l1   penalties

算法如下

（2）penalizing collisions基于 minimum translation distance，一种普遍的碰撞检测方法

轨迹优化方法分两类：1.优化用其它方法得到的轨迹，使其光滑和变短  2.从头开始规划

轨迹优化的两个主要组成部分：1.数值优化 sequential convex
optimization, with `1 penalties for equality and inequality

constraints

无碰撞的配置是高度非凸的

2.碰撞检测和惩罚  compute signed distances using convex-convex
collision detection

碰撞检测往往是计算昂贵的

优点：1.速度快，因为奇异的碰撞惩罚原则 2.可靠性高，比基于采样的规划解决更大部分的问题 3.轨迹质量高，因为将避免碰撞作为刚性约束  4.灵活性高，新的约束和成本项可以很容易的加入到问题中，因为该数值优化方法是数值鲁棒的

2.碰撞检测和惩罚

用了最小平移距离的方法，两个集合A,B的距离定义如下：

是由 Gilbert-Johnson-Keerthi (GJK)算法计算得到的

同理，定义集合A,B的穿透深度为

是由 Expanding  Polytope Algorithm (EPA)算法计算得到的

符号距离定义如下：

sd(A, B) = dist(A, B) − penetration(A, B)

要使机器人没有碰撞，要满足一下约束，定义安全界限 dsafe，有

该约束可以放松到 `1 penalty

如图所示：

带符号的距离函数是由以下规则得到的

求解过程如下：

首先作线性近似

然后求导

以上的工作是在离散轨迹的情形，这些离散的路径点最后要转化成连续时间的轨迹。

下面考虑连续时间情形下的约束：

以上得到的离散轨迹由线性插值或样条插值转化成连续轨迹有可能会碰撞，如下图：

计算如下：

扫描体A的体积：

扫描体A和障碍物B的有符号的距离：

我们不需要去计算扫描体A的凸包，因为有符号的距离成本可以由支撑映射计算得到。

支撑映射：

下面计算碰撞成本的梯度，连续时间的比离散时间的稍微复杂。

首先，令

从而，我们做一个近似

我们计算这些顶点的雅克比

关于时间t和t+1 作线性化有符号的距离sd

说明如图：