在一个实施方式中，公开了用于为自动驾驶车辆(ADV)规划轨迹的方法、设备和系统。所述方法包括：接收多个优化输入，该多个优化输入包括轨迹时长、时间离散分辨率、自动驾驶车辆(ADV)起始状态、道路形状函数、最大急动度和最大横向加速度；接收多个优化约束，该多个优化约束包括与最大急动度和最大横向加速度有关的约束；接收与优化目标相关联的成本函数，该成本函数包括与累积急动度有关的第一项、与最终纵向位置有关的第二项、与最终纵向速度有关的第三项以及与最终纵向加速度有关的第四项；利用非线性优化生成多个规划的ADV状态作为优化结果，其中，该优化结果使成本函数的值最小化；以及基于多个规划的ADV状态生成控制ADV的控制信号。