代价函数的设计
lattice¶
安全： 1.避免碰撞：和障碍物的距离
舒适 1.纵向平滑：jerk的平方和 2.向心加速度：kappa×速度平方 3.横向偏移：l的平方 4.横向舒适性
高效: 1.纵向到达目标（和参考速度的偏差）
优化方法¶
DP路径规划 安全： 1.碰撞：和障碍物的距离
舒适： 1.平滑：对l的一阶偏导，二阶偏导、三阶偏导的平方和
高效： 1.和参考轨迹的偏差
QP路径规划： 安全：无
舒适： 1.平滑：对l的一阶偏导，二阶偏导、三阶偏导的平方和
高效： 1.和参考轨迹的偏差
DP速度规划 安全：ST图上的碰撞 舒适：加速度，jerk的平方和 高效：和参考速度的偏差
QP速度规划 安全：无 舒适：加速度，jerk的平方和 高效：和参考速度的偏差