碰撞检测~OBCA

一些基础算法的回顾，这里包括很经典的一个OBCA算法，这个之前整理过论文推土机：使用混合A* 和Optimization-based method进行泊车规划的算法整理, 不过就是整理了一些论文，既然还没整理过其公式推导，不如顺便回顾完就在这里记录下。

OBCA的核心就是将碰撞检测这个非凸非光滑的问题转变成非凸的光滑问题，如果我们不考虑轨迹已经发生碰撞，必须要优化出一条尽量减少碰撞的轨迹的话，原来这个非凸非光滑问题可以直接别转化为凸的光滑问题。那么怎么做这个转变呢，基本思路就是使用对偶性质。这个对偶性质在geometry方面的应用在convex optimization中其实已经给出很多例子了。事实上，本文从两个问题的构造形式上，都给出了最后的对偶问题的转化结论。一个是将自车作为质点，障碍物进行膨胀这个前提下的对偶问题转化；另一个则是自车和障碍物都是full dimension的，比如都给包裹成多边形或多面体，然后进行问题转化。前者不好的地方是没法establish the approximation error，后面则是精准描述了碰撞，但是最大的问题是引入了大量对偶变量，如果有N个状态点和M个障碍物，则会引入M* N个对偶变量，这个优化问题规模就显著增加了不少，求起来自然会慢很多。

所以这里的自车空间，以及旋转平移矩阵写成：

回到优化问题本身，其基本形式为：

然后通过对偶性质，我们可以获得如下的等价关系：

和原来的优化问题相比，这里除了约束形式有所变化之外，还增加了非常多的优化变量：

不过根据优化问题的形式可以看到，约束还是凸的非线性约束，尚且还能用IPOPT进行求解，可以考虑一般性的问题“. Indeed, in case collisions cannot be avoided, the above formulation is not able to find "least-intrusive" trajectories by softening the constraints.”如果出现了解不得不发生碰撞，没有办法避免，我们也得给出一个尽量发生最小碰撞的解，具体表达就是尽量少的侵入障碍物的解，这个东西的表达就得用到刚才distance中的penetration部分。一样的，还是利用对偶性质，把它做一个转换：

其实这个结论教材里也写了，见到8.2节(Distance between sets ) 大概意思就是在讲如下的sets的距离怎么算：

这个问题的优化表达为：

我们用类似的思路就能证明上面的结论，现在我们也一样引入这么一个中间变量，优化问题就变成：

文章后面还有一个后续版本发到了CDC2018，主要是针对之前的工作再加了一些工程化的改动。

原创作者: whaosoft 转载于: https://blog.51cto.com/whaosoft/11642181