本文解析论文Autonomous Parking using Optimization-Based Collision Avoidance。文章来自uc berkley，这个方案也是非常主流的混合A*泊车方案中的一种，百度也借鉴了这个方案来设计自己的在U型弯和停车场泊车中的功能。文中的方案也是已经开源到了作者GitHub，但这个是julia写的，感觉只有我数学系的老师金炳义才会用Julia，我港科数学系不是盖的好吧， 给数学系丢脸了。Julia这个我是真的没接触过，好在可以直接去读apollo的c++源码如果你感兴趣的话。文章的解析只是个大概，给出了整体思路，对于细节的把握要去看原文，以及作者在文章中提到的自己的另一篇数学推导的证明。
文章的大概思路在于对混合A*计算出来的粗路径做了后处理，进行了基于优化的平滑，并且这个这个过程是有阶梯的(hierachical).所以作者推出了升级版的H-OBCA.这个方案的优势在于：第一，粗路径不一定很合理，基本都是sub optimal的，有时候还要多走很多路(在最后的simulation里面有详细对比)。第二，用基于优化的方案也是有风险的，这个优化问题是个非线性优化问题，解这类问题对初始点的选择很敏感，好在粗路径直接解决了这个问题，我们可以选择粗路径进行解非线性优化问题的热启动，于是我们既能够保证一定能解出来这个非线性优化问题，又能保证路径要远比粗路径合理的多。
首先我们要将问题建模(数学化)。
E表示的是车辆所占据的体积，R和t表示车辆在全局坐标系下发生的转动和平移。
障碍物由O表示，不发生碰撞就意味着E和O不存在交集：
如果我们定义signed distance:
通过一系列证明，这将被等价为：
相关的详细推导在作者的另一篇论文中有写。这里不多说了。
接下来定义代价函数：
tao是运行时间，然后我们还要惩罚input u, 以及 delta_u. 其中u是整个离散空间中的全部u的累加。delta_u通过查分计算获得。另外这个tao也是优化变量，因为我们不把他定下来，还可以惩罚时间，我们希望走完这些路尽量快。离散状态使用龙格库塔进行离散推算。最后就写出了带约束的非线性优化问题如下：
其中约束包括了起始点和终点两个等式约束。龙格库塔状态推算的等式约束。状态x和输入u的不等式约束，碰撞检测的不等式和等式约束。
接下来就是规划泊车曲线的三步骤，也是H-OBCA的核心：
1，混合A设计出粗路径，对于混合A的优化直接参考Hybrid A*论文解析(2)
2，每隔一段帧数做一次全局RS曲线搜索，看看能不能获得当前的不发生碰撞的到达终点的RS曲线。
3，使用粗路径作为OBCA方案的热启动，解优化问题获得优化后的平滑曲线。
接下来的仿真对比了这几个方案：
单纯的OBCA和混合A*都不及两者结合好使。
另外还特别分析了热启动对解非线性优化问题的重要性：无论是成功率还是时间，使用热启动都要优越很多：
最后提到了trajectory tracking的方案，横向tracking用了LQR,纵向用了纯P。显示误差还是很小的：
