Coursera Self-Driving Car Part4 运动规划之路径规划优化

作为Coursera第四章FinalProject的理论准备，整理了Coursera无人驾驶课程4.7.2 parametric curves的笔记。

Coursera无人驾驶课程视频链接

目录

学习目标

三次螺旋曲线及其边界条件

 位置积分和辛普森公式

螺旋线应用辛普森法则

辛普森法则的边界条件

近似的曲率约束

 弯曲能量的目标函数

 初始的优化问题

优化问题的软约束

参数的重映射

最终的优化问题

 总结

---

学习目标

识别需要的边界条件并应用于螺旋线路径规划

知道如何近似约束使得计算更简便

知道如何重映射参数来提升优化的收敛速度

三次螺旋曲线及其边界条件

这里的s代表弧长。

边界条件指明了曲线开始及结束的状态，状态包括x,y,θ,k，即x坐标，y坐标, 航向角，曲率

螺旋曲线终点位置缺少解析解，需要数值近似(辛普森公式)，参考我另一篇博文 Coursera Self-Driving Car Part4 运动规划之参数曲线笔记 的"多项式螺旋线的x,y坐标计算"小节

 位置积分和辛普森公式

总之，类似于参数螺旋线这种由积分定义的函数可以用辛普森公式近似求解。

螺旋线应用辛普森法则

取n=8，则x,y,theta, k 都可以写成弧长s的多项式形式。

辛普森法则的边界条件

起点s=0的约束条件(x0,y0,θ0,k0)实际上已经在上面的PPT里的辛普森公式里已经包含了，那么终点的约束条件(xf,yf,θf,kf)单独列出作为边界条件。

优化的参数就是曲线的弧长。

近似的曲率约束

通过采样路径上的点，限制路径上的部分采样点的曲率来达到约束整体曲线的目的。

比如说可以约束曲线的1/3,2/3处的点的曲率,

起点和终点为等式约束的边界条件。

产生螺旋线的约束就已经完备了。

 弯曲能量的目标函数

弯曲能量的计算就是曲线的曲率的平方沿着螺旋线的积分，弯曲能量决定了舒适性。

这个方程的积分很容易计算，有closed解析解。其一阶导也有解析解。

优化的目标函数就是要使曲线的弯曲能量最小，找到最优的a0,a1,a2,a3,sf。

 初始的优化问题

在这些约束下,求目标函数的最小值的点。可以将优化问题在车体坐标系下进行，就可以将起始点边界条件都变为0，已达到简化问题的目的。

优化问题的软约束

所有约束都满足对于优化器来说很难正好都满足。

可以适当的将一些等式约束，以权重很大的惩罚项，放到目标函数里，这样的结果就是优化的结果使得该等式约束可能不会严格成立，可能会很接近。

下图就是将终点的边界条件约束：x=xf,y=yf,θ=θf,k=kf变成柔性约束放到目标函数里。

参数的重映射

根据上面推到，螺旋线，要实现弯曲能量目标函数最小，要优化a0,a1,a2,a3,sf 5个参数。

现在进行重映射：

p0,p1,p2,p3分别对应曲线上4个点（0，1/3*sf , 2/3*sf,  sf）的的曲率，那么根据螺旋线的曲率表达式，四个方程四个未知数，则

a0,a1,a2,a3都可以用p0,p1,p2,p3表示，这样要优化的参数就映射成了物理意义明确的参数，p4对应曲线的长度即sf

那么要优化的参数就是p0,p1,p2,p3,p4

由于起点和终点的曲率已知，p0,p3为已知，只需优化p1,p2,p4三个参数，优化问题得到了降维。

最终的优化问题

根据上一张PPT的内容说明，将优化问题用新的参数代替得到新的优化问题的表达式。

曲率约束直接对应新参数，非常简便。

边界条件p0,p3点的曲率由软约束来处理。

注：下图中的α，β，γ为自己设定大小的目标函数惩罚项权重，越大说明越看重此项。

 总结

回顾了边界条件的状态和曲率约束

介绍了辛普森法则来计算螺旋线的位置

用弯曲能量来设计目标函数

开发了参数重映射方法，降低优化参数的维度，加快优化问题的收敛速度。