自动驾驶决策规划算法——二次规划

自动驾驶决策规划算法第二章第二节(中) 参考线算法_哔哩哔哩_bilibili

动态规划开辟的凸空间如下，两条橙色线之间：

黄色的点就意味着L的上下界，物理意义是当轨迹s=s1时，L的范围应该是(Lmin1,Lmax1)之间，这个范围就是开辟的凸空间。

规划的路径要满足：

求解L = f(s)使用Apollo3.5中的算法——分段加加速优化算法：

假设在f(s)的三阶导数恒为一个常数，那么f(s)四阶以上的导数都是0；

 上面的方程组就是二规划的等式约束；除了对Li有等式约束，还有不等式约束，不等式约束主要就是L不能超过凸空间的范围，如下：

这样近似是安全的，大角度时只会把车估计大，而不是估计小，是一个比较安全的。

约束时：

使用上面的约束方法不合适，因为上图的Lp1已经碰到凸空间的边界。需要做的保守一些，如下：

具体就是寻找车辆在凸空间上附近(si-d2,si+d1)所有lmaxi都囊括进来进行比较，选择最小值，记未ubi，使得四个脚点都要小于它，对于lmini也进行近似的处理，找到(si-d2,si+d1)所有lmini中的最小值，是四个角点都小于它。

不等式约束可以写成如下形式：

由上面可以推出总的不等式约束：

 二次规划的等式约束和不等式约束都已经写出来了，接下来就写其cost function：