碰撞检测最新算法!OBCA公式详尽推导(泊车必备)

作者 | 论文推土机  编辑 | 汽车人

原文链接:https://zhuanlan.zhihu.com/p/667368813

点击下方卡片,关注“自动驾驶之心”公众号

ADAS巨卷干货,即可获取

点击进入→自动驾驶之心技术交流群

本文只做学术分享,如有侵权,联系删文

最近在推进一些非常屌的工作,还处于严格保密阶段。涉及到一些基础算法的回顾,这里包括很经典的一个OBCA算法,这个之前整理过论文推土机:使用混合A* 和Optimization-based method进行泊车规划的算法整理, 不过就是整理了一些论文,既然还没整理过其公式推导,不如顺便回顾完就在这里记录下。

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

b4f90c8c5342bfe31b4792fd67309693.png edb1594c6e08a6ff3e98809e2d96acc7.png 1c24b5177da981b30a5d7975822a50ff.png 43bfdaababeffcc8383c0f879d80554f.png 240fe330b92a89b8386ebab7949fe701.png

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

c15f35dc5a1ba36bef57056d1a34222b.png

回到优化问题本身,其基本形式为:

19aad69124211f748f1c7a8cf66c9878.png 1fa6cea5b1e5a9d7332c31a3a594a98c.png 6c6a29b23ebfa08a0a8ebf02fa822e36.png

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

43d331676d6e1fcffe280cffb7d19fdd.png

这里的证明如下,原问题是:

c6ab35fb38bd793757cc425fd9f3eed5.png

原问题的对偶问题是:

b6552e4abd5b730b81bc2c66bb101a7a.png

由于强对偶成立,所以这里没有对偶间隙,我们可得:

29bda5f61e16a33ced1d6407d3cde6b3.png 25f31e83ce97e8ba4e5570a5fe968833.png f833e715482922c858e981681d46363e.png

有了对碰撞检测部分约束的变化,原来的优化问题就变成了:

d1e9b357392ec142cc53e0abe589e991.png

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

ba1553994b45a35b908468abfac14bc5.png

不过根据优化问题的形式可以看到,约束还是凸的非线性约束,尚且还能用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部分。一样的,还是利用对偶性质,把它做一个转换:

975a6770a44aa057f25f98dcc383b3dd.png 9a4d880eab24caf583e4aed27e316c21.png 11d66c583ef0e2c3d52a2c48a7c14e07.png

优化问题的general表达为:

24f04743bd587829b1e273b71675db1d.png 425d5a3b685800f615a3d278dc169525.png d094a260ed7e0e01db6658f639135ce4.png

然后我们再看full dimension形式的问题表达下面的优化问题转换,结论如下:

856d5d286d9dbd2b89390c4a205ddc8c.png

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

24b09ea9a2cd3e022983edb153cabb9b.png

这个问题的优化表达为:

110f4332d1449cc3cdfa3072a12591eb.png

对偶函数是:

d7acb82e4778310cc2f5be40b56491a3.png

对偶问题就是:

138b0cace170505a9c6d90e132a87c65.png

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

f9e4064c0b28756de60d450bfa471d11.png

对偶函数是:

9ab2df2f7cb69c9974153fff027ab100.png ab9ae48a9d46fc8e4f243594ab88f621.png a4580f645dc1b2238855489840d48a03.png 2279e539546b9787ef26e6ea958f3ca6.png

这个表达就是刚才的对偶转化的结论:

68376543c28c3ac558f676d432d67dfd.png

由于强对偶成立,所以只要能够找到满足条件的就可以保证安全距离约束成立:

f6a546967af0531ba6bb453b76aa797e.png

这样当前的优化问题可以被重写成:

2c1f4b2d655fcbf98a0a62578eb03882.png

然后继续考虑general 的表达,把penetration也考虑进来:

e4837277cabcfcdd1e38383c2f4e6007.png

然后对偶转化部分的一般化表达就变成了:

8d28ee46cdd3a15d21b116222477f829.png 6b8d0526efbb27bc4651022a4ec1138c.png

然后在考虑一个松弛变量,将碰撞部分的约束进行一个松弛:

c1d29789a77298d7bc0748bd303a2d9a.png

最后优化问题的表达为:

615f5092d2840772d33ecbe646eceec7.png

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

e34ff4eaf6b54e2276c830a78d30ef63.png 3f17dabe5889a5619e7804c54075298f.png 062e39609e334b51838412d32e4006cd.png 9b9c30ae96a3a1818f26a72681cbcea0.png 63b88bb51f59a593274a405b85d5833f.png

① 全网独家视频课程

BEV感知、毫米波雷达视觉融合多传感器标定多传感器融合多模态3D目标检测点云3D目标检测目标跟踪Occupancy、cuda与TensorRT模型部署协同感知语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频(扫码即可学习

17dc6f6b2d51b7ae362ec6273ecf01e8.png 视频官网:www.zdjszx.com

② 国内首个自动驾驶学习社区

近2000人的交流社区,涉及30+自动驾驶技术栈学习路线,想要了解更多自动驾驶感知(2D检测、分割、2D/3D车道线、BEV感知、3D目标检测、Occupancy、多传感器融合、多传感器标定、目标跟踪、光流估计)、自动驾驶定位建图(SLAM、高精地图、局部在线地图)、自动驾驶规划控制/轨迹预测等领域技术方案、AI模型部署落地实战、行业动态、岗位发布,欢迎扫描下方二维码,加入自动驾驶之心知识星球,这是一个真正有干货的地方,与领域大佬交流入门、学习、工作、跳槽上的各类难题,日常分享论文+代码+视频,期待交流!

a24372a8687448f80364f8d470ce53e0.png

③【自动驾驶之心】技术交流群

自动驾驶之心是首个自动驾驶开发者社区,聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、点云处理、端到端自动驾驶、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群,备注:学校/公司+方向+昵称(快速入群方式)

f38adafc7d07d1ce6ee045185631bfaf.jpeg

④【自动驾驶之心】平台矩阵,欢迎联系我们!

0be2de31e12255391a5572c40dece242.jpeg

内容简介  《实时碰撞检测算法技术》详细阐述了与碰撞检测问题相关的高效解决方案及相应的数据结构和算法,主要包括:碰撞检测系统中的设计问题、数学和几何学入门、包围体、基本图元测试、层次包围体技术、空间划分、BSP树层次结构、凸体算法、基于GPU的碰撞检测、数值健壮性、几何健壮性以及优化操作。另外,《实时碰撞检测算法技术》还提供了相应的算法、代码以及伪代码,以帮助读者进一步理解计算方案的实现过程。  《实时碰撞检测算法技术》适合作为高等院校计算机及相关专业的教材和教学参考书,也可作为相关开发人员的自学教材和参考手册。第1章 概述1.1 内容概览1.2 关于本书的代码第2章 碰撞检测系统中的设计问题2.1 碰撞算法的设计因素2.2 应用程序中对象的表达方式2.3 查询类型2.4 环境模拟参数2.5 性能2.6 健壮性2.7 实现与使用的简洁性2.8 小结第3章 数学和几何学入门3.1 矩阵3.2 坐标系统和顶点3.3 向量3.4 质心坐标3.5 直线、光线和线段3.6 平面和半空间3.7 多边形3.8 多面体3.9 凸包计算3.10 域3.11 Minkowski和与Minkowski差3.12 小结第4章 包围体4.1 BV期望特征4.2 轴对齐包围盒4.3 Spheres球体4.4 方向包围盒4.5 球扫掠体4.6 半空间相交体4.7 其他类型的包围体4.8 小结第5章 基本图元测试5.1 最近点计算5.2 图元测试5.3 直线、光线和有向线段的相交测试5.4 其他类型的测试5.5 动态相交测试5.6 小结第6章 层次包围体技术6.1 层次结构设计问题6.2 层次结构的构建策略6.3 层次结构的遍历6.4 包围体层次结构示例6.5 合并包围体6.6 高效的树型表达方式及遍历6.7 通过缓存机制改善查询6.8 小结第7章 空间划分第8章 BSP树层次结构第9章 凸体算法第10章 基于GPU的碰撞检测第11章 数值健壮性第12章 几何健壮性第13章 优化操作参考文献
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值