自动驾驶の泊车算法

阅读量：

自动驾驶泊车算法学习记录

在学习自动驾驶泊车方面的算法，看的论文主要有以下三篇，分别是Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments， DL-IAPS and PJSO: A Path/Speed Decoupled Trajectory Optimization and its Application in Autonomous Driving， Speed Profile Planning in Dynamic Environments via Temporal Optimization

Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments

这篇论文应该只要是学习Hybrid Astar的人都看过，主要讲的是在开放空间搜索泊车路径，第一步先搜索出一条大概满足运动学约束的路径，然后密集插值，在进一步采用CG算法优化平滑度，保持原来关键的位置不变，这样也不用害怕碰撞了，这篇论文进一步求路径点对应的速度

DL-IAPS and PJSO: A Path/Speed Decoupled Trajectory Optimization and its Application in Autonomous Driving

这篇论文是逻辑上和上一篇论文挺像的，先搜索一条路径，然后对路径点进一步平滑，这个论文平滑的时候会采用一个信赖半径，发生碰撞了就缩小关键点的可移动范围，除此之外DL-IAPS and PJSO 还求解了路径的速度，这是比较牛逼的，但我看不太懂- -，后续有机会再进一步研究

Speed Profile Planning in Dynamic Environments via Temporal Optimization

思路

我看传统的泊车方法好像都需要先搜索出一条路径，然后再作进一步的处理，我觉得Hybrid Astar的搜索速度非常依赖于地图分辨率，分辨率小了就会很慢，大了就没法精确避障了，如果直接使用一条直线段连接成的路径作为参考线，不用搜索的结果了，然后对这个直线段采用最优化的方案进行求解效果会如何。TEB就是直接对直线段进行优化，并且也实现了car-like方面的路径规划，于是我去下载了源码然后跑了一下，发现效果不太好，速度很慢，很容易出现无解的情况，我觉得可能是因为TEB还多了一个时间变量需要处理，时空联合规划很容易失败，如果我把这个时间给去掉，在进行优化的话效果会不会就不一样了？

方法

vehicle with trailer kinematic 参考文献

code

代码方面就是把TEB的代价函数复制过来就可以了，优化器改成ceres-solver的，车辆模型，我不仅实现了汽车模型，还顺带在卡车模型啥也测试了一波。

Result

测试模型包含阿克曼汽车模型以及vehicle with trailer模型，图中阿克曼就是用一个矩形来描述，卡车模型则用两个矩形来描述，小个的是车头，长条的是车厢（用c++画的图，很简陋- -），绿色框是辅助划线的可以不用管，其他的多边形则为障碍物，

我以为测试结果会很差，结果还是有点出乎意料的，虽然有的时候会出现无解，但是只要初始线段给的好一点就不会出问题，优化的速度也挺快平均时间大概是0.3s，路径的质量的评价我觉得主要是看平滑度和曲率大小，只要曲率不会超过车辆最小转弯半径就可以了，这里我设置的曲率最大是0.155，列了几组测试图，第一个是直线段参考线和优化后的路径图，第二幅是对应的曲率和航向 whaosoft aiot http://143ai.com

Test1