基于高阶运动学模型与 Constrained iLQR/DDP 的自动驾驶轨迹规划算法

论文 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 }]$，其中：

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

在每个离散的时间步 $T_r$ 内，假定 $j$ 和 $\ddot{\kappa }$ 都是恒定的，则该时间步内走过的距离 $S_r=v_0{T}_r+\frac{1}{2}{a}_0{T}_r^2+\frac{1}{6}{j}_0{T}_r^3$，平均曲率 $\bar{\kappa }=\frac{1}{T_r}{\int }_0^{T_r}\kappa (t)dt={\kappa }_0+\frac{1}{2}{\dot{\kappa }}_0{T}_r+\frac{1}{6}{\ddot{\kappa }}_0{T}_r^2$。

我们假设航向角的变化量与行驶距离成正比，则 $\frac{d\theta }{ds}=\bar{\kappa }$

运动学模型如下：

$$\begin{array}{rlrl}& x_1& & =x_0+{\int }_0^{S_r}\cos ({\theta }_0+\bar{\kappa }s)ds\\ & & & \begin{array}{r}=x_0+\frac{\sin \left({\theta }_0+\bar{\kappa }{S}_r\right)-\sin {\theta }_0}{\bar{\kappa }}(\bar{\kappa }\ne 0)\end{array}\\ & & & =x_0+S_r\cos {\theta }_0(\bar{\kappa }=0)\\ & y_1& & \begin{array}{r}=y_0+{\int }_0^{S_r}\sin ({\theta }_0+\bar{\kappa }s)ds\end{array}\\ & & & \begin{array}{r}=y_0+\frac{-\cos ({\theta }_0+\bar{\kappa }{S}_r)+\cos {\theta }_0}{\bar{\kappa }}(\bar{\kappa }\ne 0)\end{array}\\ & & & =y_0+S_r\sin {\theta }_0(\bar{\kappa }=0)\\ & v_1& & =v_0+a_0{T}_r+\frac{1}{2}{j}_0{T}_r^2\\ & {\theta }_1& & ={\theta }_0+{\int }_0^{S_r}\kappa (s)ds={\theta }_0+{\int }_0^{T_r}\kappa (t)v(t)dt\\ & {\kappa }_1& & ={\kappa }_0+{\dot{\kappa }}_0{T}_r+\frac{1}{2}{\ddot{\kappa }}_0{T}_r^2\\ & {\dot{\kappa }}_1& & ={\dot{\kappa }}_0+{\ddot{\kappa }}_0{T}_r\end{array}$$

松弛障碍函数

CILQR 算法把每一项约束表示为一个代价函数项，从而使得初始轨迹不可行时，也能对轨迹进行迭代优化。文章对约束 $g(x,u)$ 的代价计算采用了松弛对数障碍函数：

$${\beta }_{relax}(g)=\{\begin{array}{ll}-\frac{1}{t}log(-g)& (g<-ϵ)\\ \frac{k-1}{tk}[(\frac{-g-kϵ}{(k-1)ϵ}{)}^k-1]-\frac{1}{t}logϵ& (g\ge -ϵ)\end{array}$$

对于该函数中的参数，文章选择 $k=2$ 来保证它是 $C^2$ 型函数（即对任意的 $ϵ$ 和 $t$ 都大于 $0$）。对于所有不等式约束，文章选择 $t=5$。而对于 $ϵ$，则根据不同的约束，取不同的值。

代价函数

1. Lateral Acceleration

横向加速度：
$$a_y=\kappa v^2$$

横向加速度的代价函数项：
$$cos{t}^{a_y}=w_{a_y}\cdot {a_y}^2$$

2. Lateral Jerk

横向 Jerk：
$$j_y=\frac{\mathrm{d}}{\mathrm{d}t}\left(v^2\kappa \right)=2v\kappa a_x+\frac{v^2}{L}{\sec }^2(\delta )\dot{\delta }=2v\kappa a_x+v^2\dot{\kappa }$$

横向 Jerk 的代价函数项：
$$cos{t}^{j_y}=w_{j_y}\cdot {j_y}^2$$

3. Longitudinal Jerk

纵向 Jerk 的代价函数项：
$$cos{t}^j=w_j\cdot j^2$$

4. Direction Tracking

当前车辆航向角与道路方向的误差的平方作为方向追踪的代价函数项：

$$cos{t}^{\theta }=w_{\theta }\cdot ({\theta }_k-{\theta }_k^r{)}^2$$

约束

1. 车辆动力学约束
2. 道路的边界约束
3. 碰撞约束