基于高阶运动学模型与 Constrained iLQR/DDP 的自动驾驶轨迹规划算法_integrating higher-order dynamics and roadway-comp-CSDN博客

本文为博主原创作品，著作权归作者所有，转载时请务必在显著位置以超链接方式标明本文出处、作者信息和本声明，未经作者允许不得用于商业目的，否则将追究法律责任。

本文链接：https://blog.csdn.net/no_xycoordinate/article/details/138821116

论文 Integrating Higher-Order Dynamics and Roadway-Compliance into Constrained ILQR-based Trajectory Planning for Autonomous Vehicles 介绍了当前基于 iLQR/DDP 的自动驾驶轨迹规划算法大多运用的是单车模型或简单的线性模型，这样对车辆运动学模型的粗略近似，对于微分动态规划等敏感度较高的优化算法来说，最终产生的轨迹不能很好的满足安全性和舒适性，使得实际工程化落地效果并不理想。

针对这种简单模型的局限性，文章引入了高阶运动学模型，对约束的处理则运用了松弛的对数障碍函数（relaxed logarithmic barrier function），基于该高阶运动学模型进行带约束的 iLQR 轨迹规划，能更好的表达和设计更为丰富多样的代价函数以及约束条件，最终在复杂场景下收到更好的效果。

高阶运动学模型

文章对车辆运动学模型引入了高阶控制项，状态量是 $[x,y,v,\theta,a,\kappa,\dot{\kappa}]$ ，控制量是 $[j,\ddot{\kappa}]$ ，其中：

$(x,y,\theta)$ 描述了车辆的二维坐标以及车辆的航向角。
$v$ 是车辆的纵向速度。
$a$ 是车辆的纵向加速度。
$\kappa$ 是车辆的曲率。
$\dot{\kappa}$ 是车辆的曲率对时间 $t$ 的一阶导。
$j$ 是纵向 jerk。
$\ddot{\kappa}$ 是车辆的曲率对时间 $t$ 的二阶导。

在每个离散的时间步 $T_r$ 内，假定 $j$ 和 $\ddot{\kappa}$ 都是恒定的，则该时间步内走过的距离 $S_r = v_0T_r + \frac12a_0T_r^2+\frac16j_0T_r^3$ ，平均曲率 $\bar{\kappa} = \frac{1}{T_r}\int_0^{T_r}\kappa(t)dt=\kappa_0+\frac12\dot{\kappa}_0T_r+\frac16\ddot{\kappa}_0T_r^2$