您当前所在位置：智能汽车 > 查看内容

碰撞检测最新算法！OBCA公式详尽推导（泊车必备）

发布：xiaocheng 来源：

PostTime：10-12-2023 08:16

碰撞检测最新算法！OBCA公式详尽推导（泊车必备）

以下为文章全文：(本站微信公共账号:cartech8)

汽车零部件采购、销售通信录 填写你的培训需求，我们帮你找 招募汽车专业培训老师

最近在推进一些非常屌的工作，还处于严格保密阶段。涉及到一些基础算法的回顾，这里包括很经典的一个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部分。一样的，还是利用对偶性质，把它做一个转换：