约束迭代LQR在自动驾驶中的应用

约束迭代LQR（CILQR）在道路自动驾驶运动规划中的应用

I. 引言

运动规划是自动驾驶中最关键的模块之一。典型的自动驾驶车辆运动规划器会考虑来自环境的信息（例如主车及周围车辆的运动状态），并为车辆规划一条待执行的轨迹。尽管运动规划已研究数十年，设计一个同时满足以下要求的运动规划器仍具挑战性：（1）计算需具备实时性，以应对动态环境和紧急情况；（2）规划需具备时空性，以应对动态障碍物；（3）规划器需能够处理复杂约束，如避障。

自动驾驶运动规划技术大致可分为三类：基于搜索的方法、基于采样的方法和基于优化的方法[1],[2]。

基于搜索的方法通常指A* [3]搜索或D* [4]搜索。它们的规划结果是全局最优的，但在处理移动障碍物时算法效率较低，且难以进行时空规划。此外，这类方法也不适用于道路环境下的驾驶运动规划，因为很难设定目标并设计良好的启发式函数，而这会显著影响A*搜索的计算时间，从而限制其实时性。基于采样的方法主要以快速探索随机树（RRT）[5]为代表。RRT的主要缺点是在进行碰撞检测时效率较低，此外其规划结果为次优解。

无论是基于搜索的还是基于采样的方法，都是在离散空间（无论状态空间或动作空间）中进行规划，这会导致轨迹不平滑。同时，由于“维度灾难”，离散空间的分辨率将极大地影响计算效率。

基于优化的方法将运动规划建模为一个数学优化问题。规划结果是连续的、最优的且具有时空性的。然而，由于车辆动力学模型是非线性的，且避障约束是非凸的，该问题包含非线性等式约束和非凸不等式约束，使得求解该优化问题非常困难且计算效率低下。该领域的最新工作由梅赛德斯‐奔驰[8]完成，其利用序列二次规划（SQP）求解非线性和非凸问题，但计算时间约为0.5秒，仍需缩短。一些研究将空间和时间规划解耦 [6],[7]，这些方法具有较高的计算效率。然而，时间规划仅能在空间规划的基础上进行局部调整，因此并非真正的时空联合规划。

迭代线性二次调节器（ILQR）[9],[10]是一种基于线性二次调节器（LQR）的算法，能够求解受非线性系统动力学约束的无约束最优控制问题。它最重要的优势是时间效率，这使其成为解决复杂优化问题中实时问题的潜在方法。然而，很少有文献在ILQR中考虑约束，而约束在自动驾驶运动规划中是不可避免的。受限控制微分动态规划（DDP）在[11]中被提出，用于求解带约束的非线性最优控制问题，但其约束形式仅限于控制输入的上下界。扩展LQR [12],[13]实现了避障功能，但它仅通过惩罚主机器人中心到障碍物中心的距离来处理圆形障碍物。然而，在自动驾驶中，障碍物的形状可能更为复杂。据我们所知，尚无研究探讨如何使用ILQR处理通用形式的约束。因此，目前还没有人使用ILQR来处理带有复杂约束的道路自动驾驶运动规划问题。

本文提出了一种约束迭代LQR（CILQR），用于处理ILQR的通用形式的约束。LQR通过动态规划（DP）求解，该方法非常高效，但只能处理无约束问题。为了利用动态规划的效率，通过添加障碍函数将约束转化为成本函数。非线性和非凸约束将被线性化。障碍函数随后被二次化，并加入原始成本函数中。然后将CILQR算法应用于一般的公路自动驾驶运动规划问题，其中使用椭圆开发了一种新的障碍物通用表示方法。中心线采用多项式进行逼近，从而实现连续可微距离函数，以获得更好的收敛性。

本文的其余部分组织如下。第二节介绍了约束迭代线性二次调节器算法。第三节展示了该算法如何应用于一般的道路自主驾驶运动规划问题。第四节给出了若干典型场景的仿真结果。最后，第五节对全文进行总结。

II. 约束迭代线性二次调节器

A. 问题描述

问题1 （一个通用的运动规划问题）。可以将约束滚动时域运动规划问题表述为：

$$
\min_{u_0,…,u_{N−1}, x_0,…,x_N} J= c_N(x_N)+ \sum_{k=0}^{N−1} c_{x_k}(x_k)+c_{u_k}(u_k)
$$

s.t.
$$
x_{k+1}= f(x_k, u_k), \quad k= 0, 1,…, N − 1 \tag{1a}
$$
$$
x_0= x_{\text{start}} \tag{1b}
$$
$$
f_{x_k}(x_k) ≤ 0, \quad k= 0, 1,…, N \tag{1c}
$$
$$
f_{u_k}(u_k) ≤ 0, \quad k= 0, 1,…, N − 1 \tag{1d}
$$

其中，$x_k$是状态， $u_k$是控制输入，下标表示时间步。$N$是预览时域。$J$是成本函数，其中$c_N$是终端阶段代价， $c_k$是时间步 $k$的代价。(1a)是系统动力学方程，可以是非线性的；(1b)是初始状态；(1c)和(1d)是状态和控制约束，可以是非线性和非凸的。这里我们假设在每个时间步，约束函数对 $x$和 $u$是解耦的。

这种通用的公式涵盖了运动规划中的大量问题。求解上述非线性、非凸优化问题的一种前沿技术是序列二次规划（SQP），它将原始问题分解为一系列二次规划 (QP)问题并进行迭代求解。然而，SQP的设计目标更为通用，适用于一般的优化问题，因此在实时运动规划中计算效率不够高。对于上述运动规划问题，由于系统动力学是唯一的等式约束，可以利用该特殊结构，通过 ILQR来提高计算效率。

B. 迭代LQR(ILQR)

问题2 （标准ILQR问题）ILQR算法可以处理以下非线性系统运动规划问题

$$
\min_{u_0,…,u_{N−1}, x_0,…,x_N} J= \frac{1}{2} x^T_N Q_N x_N + p^T_N x_N + q_N + \sum_{k=0}^{N−1}\left(\frac{1}{2} x^T_k Q_k x_k + p^T_k x_k + \frac{1}{2} u^T_k R_k u_k + r^T_k u_k + q_k\right)
$$

s.t.
$$
x_{k+1}= f(x_k, u_k), \quad k= 0, 1,…, N − 1 \tag{3a}
$$
$$
x_0= x_{\text{start}} \tag{3b}
$$

其中 $Q_N$、 $p_N$、 $q_N$、 $Q$、 $p$、 $R$、 $r$、 $q$ 是描述二次成本函数的参数。动力学方程 (3a) 可以为非线性。从一个初始控制输入序列（例如 $u_k= 0$）开始，该算法通过两个步骤进行迭代：

Step1 （前向仿真）。将当前迭代的控制序列 $u_k$ 输入到实际系统动力学(3a)中，得到本次迭代的状态序列 $x_k$。

步骤2 （反向优化）。首先，将非线性系统 (3a) 在 $x_k$ 和 $u_k$ 附近进行线性化：

$$
\delta x_{k+1}= A_k \delta x_k + B_k \delta u_k \tag{4}
$$

其中$A_k= D_x f(x_k, u_k)$和 $B_k= D_u f(x_k, u_k)$是原始动力学函数的一阶导数。现在在此迭代下并结合当前的线性化系统，该问题变为一个标准的LQR问题：

问题3 （标准LQR问题）

$$
\min_{u_0’,…,u_{N−1}’, x_0’,…,x_N’} J= \frac{1}{2} \delta x^T_N Q_N \delta x_N + \delta x^T_N p_N + q_N + \sum_{k=0}^{N−1} \left( \frac{1}{2} \delta x^T_k Q_k \delta x_k + \delta x^T_k p_k + \frac{1}{2} \delta u^T_k R_k u_k + \delta u^T_k r_k + q_k \right)
$$

s.t.
$$
\delta x_{k+1}= A_k \delta x_k + B_k \delta u_k, \quad k= 0, 1,…, N −1 \tag{6a}
$$
$$
x_0= x_{\text{start}} \tag{6b}
$$

该问题可以使用标准的LQR算法求解。注意，我们使用状态与控制的微分形式$\delta x_k$和 $\delta u_k$，以保持与ILQR中符号的一致性。在求解LQR时，应将其视为正常的状态与控制。

最优解$\delta u_k$将用于更新当前的控制序列：
$$
u_k \leftarrow u_k + \delta u_k
$$

ILQR算法细节可在[9],[10]中找到。如第一节所述，ILQR是一种求解带非线性动力学约束的运动规划问题的非常高效的算法。然而，ILQR的一个主要缺点是无法处理复杂约束，而复杂约束在大多数机器人运动规划场景中至关重要。以自动驾驶为例，存在各种用于避障、加速度和交通规则的约束，例如限速。若缺乏处理约束的能力，ILQR的功能将受到显著限制。

C. 约束迭代LQR（CILQR）

CILQR算法旨在通过扩展ILQR算法来求解问题1。

从问题2扩展到问题1存在两个额外问题：第一，问题1 中包含状态和控制约束；第二，问题1中的成本函数为一般形式，而问题2中的成本函数是二次的。CILQR采用与ILQR相同的框架，但在反向优化步骤中，将约束和代价转化为二次成本项，从而使问题具有与问题3相同的形式，并可应用标准解法。以下4个步骤描述了基于当前标称状态和控制$x_k$、 $u_k$的变换过程。由于 $x_k$和 $u_k$在我们的问题中是对称的，因此我们仅讨论 $c_{x_k}$和$f_{x_k}$，然后同样的方法适用于 $c_{u_k}$和 $f_{u_k}$。

step 1 （成本函数二次化）。如果成本函数$c_{x_k}$不是二次形式，则需要在$x_k$附近对其进行二次化。二次化过程即计算该函数的二阶泰勒级数近似：

$$
c_{x_k}(x_k+ \delta x_k) \approx (\delta x_k)^T \nabla^2 c_{x_k}(x_k)\delta x_k + (\delta x_k)^T \nabla c_{x_k}(x_k)+ c_{x_k}(x_k) \tag{7}
$$

其中 $\nabla c_{x_k}(x_k)$ 和 $\nabla^2 c_{x_k}(x_k)$ 是 $c_{x_k}$ 关于 $x_k$ 的一阶和二阶导数

step2 （约束函数线性化）。如果约束 $f_{x_k} \leq 0$ 不是线性的，则需要对其进行线性化。线性化过程是用其一阶泰勒级数近似来替代 $f_{x_k}$：

$$
f_{x_k}(x_k+ \delta x_k) \approx (\delta x_k)^T \nabla c_{x_k}(x_k)+ c_{x_k}(x_k) \tag{8}
$$

其中 $\nabla c_k(x_k)$ 是 $f_{x_k}$

step 3 （障碍函数成形）。线性化后的约束函数 $f_{x_k}$ 将通过一个障碍函数进行成形：

$$
b_{x_k} = q_1 \exp(q_2 f_{x_k}) \tag{9}
$$

其中 $q_1$ 和 $q_2$ 是用于调节障碍函数形状的参数。使用障碍函数的目的是近似指示函数，从而将约束问题转化为无约束问题。指数障碍函数 (9) 是二阶可微的，且其导数易于计算。

step 4 （约束函数二次化）。成形后的约束函数将被二次化：

$$
b_{x_k}(x_k + \delta x_k) \approx \delta x^T_k \nabla^2 b_{x_k}(x_k) \delta x_k + \delta x^T_k \nabla b_{x_k}(x_k) + b_{x_k}(x_k) \tag{10}
$$

通过应用链式法则，我们得到：

$$
\nabla b_{x_k}(x_k) = q_1 q_2 \exp(q_2 c_{x_k}(x_k)) \nabla c_{x_k}(x_k) \tag{11}
$$

and
$$
\nabla^2 b_{x_k}(x_k)= q_1 q_2^2 \exp(q_2 c_{x_k}(x_k)) \nabla c_{x_k}(x_k)(\nabla c_{x_k}(x_k))^T \tag{12}
$$

最后，得到的(7)、(10)以及它们对于$\delta u_k$的对应项被添加到问题3的成本函数(5)中。

有限差分是一种计算导数的常用方法。然而，我们建议解析地推导导数和海森矩阵，因为有限差分的时间效率较低。事实上，我们采用这一四步方案的原因之一就是为了便于获得解析导数函数。

III. 基于约束迭代LQR的公路自动驾驶运动规划

A. 问题描述

问题4 （公路自动驾驶运动规划问题）

$$
\min_{u_0,…,u_{N−1}, x_0,…,x_N} J= \frac{1}{2} x^T_N Q_N x_N + p^T_N x_N + q_N + \sum_{k=0}^{N−1} \left{ \frac{1}{2} x^T_k Q_k x_k + p^T_k x_k + \frac{1}{2} u^T_k R_k u_k + r^T_k u_k + q_k \right}
$$

s.t.
$$
x_{k+1}= f(x_k, u_k) \tag{13a}
$$
$$
x_0= x_{\text{start}} \tag{13b}
$$
$$
x_k \in C/\cup O_j(k), \quad k= 1,…, N \tag{13c}
$$
$$
u_k \in \Omega, \quad k= 0, 1,…, N − 1 \tag{13d}
$$

其中，(13a) 是非线性的系统动力学方程。避障约束是障碍物所占据空间的补集，表示为 $x_k \in C/\cup O_j(k)$，其中 $C$ 是整个空间， $O_j(k)$ 是在时间步 $k$ 时障碍物 $j$ 所占据的空间。由于 $O_j(k)$ 通常是凸的，因此 (13c) 通常是一个非凸不等式约束。

问题4是问题1的一个子问题。采用CILQR算法来求解该问题。其解将是一系列最优控制指令 $u_0, u_1,…, u_{N−1}$，以及一条规划轨迹$x_0 , x_1 ,…, x_N$。

B. 车辆模型

本文采用了图1所示的车辆运动学模型。状态为
$$
x=[ p_x, p_y, v, \theta]^T
$$
其中 $p_x$和 $p_y$表示车辆的二维位置，$v$为车辆速度， $\theta$表示偏航角。车辆动力学为：

$$
\dot{x}=
\begin{bmatrix}
v \cos\theta \
v \sin\theta \
0 \
0
\end{bmatrix}
+
\begin{bmatrix}
0 & 0 \
0 & 0 \
1 & 0 \
0 & 1
\end{bmatrix}
\begin{bmatrix}
\dot{v} \
\dot{\theta}
\end{bmatrix}
= g(x, u) \tag{14}
$$

注意，动力学方程中存在非线性项，且其中$\dot{\theta}= \frac{v}{L} \tan\delta$，其中 $\delta$是前轮的转向角。

由于我们在离散时间域中讨论，该模型需要转换为离散时间形式。一旦确定了$x_k$和$u_k$ ，我们有$t \in[T_s k, T_s(k+1)]$，
$$
x_k= [ p_{x_k}, p_{y_k}, v_k, \theta_k]^T, \quad u_k= [ \dot{v}_k, \dot{\theta}_k]^T
$$
$(T_s k)$。因此 $\dot{x}(t)=[ v(t) \cos \theta(t), v(t) \sin \theta(t), \dot{v}_k, \dot{\theta}_k]^T$，其中 $\dot{v}(t)= v_k + \dot{v}_k(t - T_s k)$且 $\theta(t)= \theta_k + \dot{\theta}_k(t - T_s k)$。代入 $v(t)$和 $\theta(t)$可得：

$$
\dot{p}_x(t)=(v_k + \dot{v}_k(t - T_s k)) \cos(\theta_k + \dot{\theta}_k(t - T_s k)) \
\dot{p}_y(t)=(v_k + \dot{v}_k(t - T_s k)) \sin(\theta_k + \dot{\theta}_k(t - T_s k)) \tag{15}
$$

积分后得到：

$$
p_x(t)= p_{x_k} + \int_{T_s k}^{t} \dot{p} x(\tau) d\tau \
p_y(t)= p {y_k} + \int_{T_s k}^{t} \dot{p}_y(\tau) d\tau \tag{16}
$$

因此，当 $\dot{\theta} k \neq 0$时，下一步时间的状态为 $p {x_{k+1}} = \dot{p} x((k+1) T_s)$和 $p {y_{k+1}} = \dot{p}_y((k+1) T_s)$。对于 $\dot{\theta}_k= 0$，我们有：

$$
p_{x_{k+1}} = p_{x_k} + \cos\theta_k(v_k T_s + \frac{\dot{v} k}{2} T^2_s) \
p {y_{k+1}} = p_{y_k} + \sin\theta_k(v_k T_s + \frac{\dot{v}_k}{2} T^2_s) \tag{17}
$$

and:

$$
v_{k+1}= v_k + \dot{v} k T_s \
\theta {k+1} = \theta_k + \dot{\theta}_k T_s \tag{18}
$$

(17) 和 (18) 可以整合为离散系统动力学模型：

$$
x_{k+1} = [ p_{x_{k+1}}, p_{y_{k+1}}, v_{k+1}, \theta_{k+1}]^T = f(x_k , u_k) \tag{19}
$$

C. 目标函数

目标函数可以解耦为四项：

$$
J= \sum_{k=0}^{N} c^{\text{acc}}_k + c^{\text{yawr}}_k + c^{\text{off}}_k + c^{\text{vel}}_k \tag{20}
$$

其中，这四项分别表示对加速度、偏航率、与参考值的偏差以及速度差的惩罚项。我们将在下文描述它们的具体计算方法：

1) 加速度

该项：

$$
c^{\text{acc}}_k = w^{\text{acc}} u^T_k \begin{bmatrix} 1 & 0 \ 0 & 0 \end{bmatrix} u_k \tag{21}
$$

是加速度的惩罚项，与乘客舒适度密切相关。超过 1m/s²的较大加速度将导致明显的不适[14]。在目标函数中加入此项可以提高乘客舒适度，调整权重$w^{\text{acc}}$可以改变舒适程度。

2) 偏航率

该项：

$$
c^{\text{yawr}}_k = w^{\text{yawr}} u^T_k \begin{bmatrix} 0 & 0 \ 0 & 1 \end{bmatrix} u_k \tag{22}
$$

是偏航率的惩罚项。它用于惩罚车辆方向的快速变化以及与驾驶安全和舒适性相关的侧向加速度。调节权重 $w^{\text{yawr}}$可以改变我们对上述问题的关注程度。

3) 参考偏差

项$c^{\text{off}} k$用于惩罚到参考路径的距离。值得思考的是我们如何定义参考路径和距离。一种广泛使用的方法是给出一系列参考点
$$
x_r = [x {r_0}, x_{r_1}, …, x_{r_N}]
$$
其中较小的下标代表较早的时间戳。该项可表示为

$$
c^{\text{off}} k = (x_k - x {r_k})^T Q^r_k (x_k - x_{r_k}) \tag{23}
$$

然而，这种表述方式可能导致一些问题场景。例如，如图2所示，车辆处于弯道上，并且需要在A线之前停车。参考$x_{r_i}$是道路中心线上的点。如果我们设置代价为(23)，则规划轨迹将类似于图中的 $x_i$。注意， $x_i$、 $x_{i+1}$和 $x_{i+2}$显著偏离了中心线，因为它们被对应的参考点$x_{r_i}$、 $x_{r_{i+1}}$和 $x_{r_{i+2}}$“拉向”这些点。

除了这些有问题的场景外，(23) 要求我们构建一个强大的上层规划器以提供充分的参考。理想的代价是惩罚规划轨迹中某一点到整个参考线的距离，而不是参考点。在这种情况下，我们无需提供一系列参考点，而只需一条简单的参考线，例如道路的中心线。奔驰 [8] 曾尝试通过构建距离场来解决此问题。然而，该距离场并非连续可微，存在不收敛的风险。

为了解决上述问题，我们使用多项式来表示参考线，并惩罚规划轨迹中各点到该多项式的距离。如图3所示，采用一个三阶多项式 $S$作为参考轨迹，$x_k$是规划轨迹中的一个点。为了求得$x_k$到曲线 $S$的距离，我们需要找到 $S$上的一个点$s_k$，使得 $\overrightarrow{x_k s_k} \perp l_k$，其中 $l_k$是经过$s_k$的切线。然后 $x_k$到曲线 $S$的距离为
$$
d(x_k, S) = | \overrightarrow{x_k s_k} |
$$
其一阶近似为
$$
d(x_k + \delta x_k, S) \approx d(x_k + \delta x_k, l_k)
$$
其平方构成一个二次项，将被设为$c^{\text{off}}_k$。

使用此方法有两个主要优点。首先，多项式是连续可微的，且其切线易于计算。其次，在实际应用中，商用传感器通常将车道拟合为多项式，例如Mobileye将车道拟合为三阶多项式。

4) 速度差

$c^{\text{vel}} k$ 是帮助我们保持相对较高速度以实现效率的代价。如图3所示，我们设定
$$
c^{\text{vel}}_k = w^{\text{vel}} ( | v {k|} | - v_{\text{des}} )^2
$$
其中$v_{k|}$是沿切线方向 $l_k$的速度，$v_{\text{des}}$是期望速度，为一个标量。

D. 约束

除了需要优化的一些目标外，驾驶时还需要遵守一些约束。本文考虑了四种约束：加速度约束、横摆角速度约束、边界约束和避障约束。

1) 加速度约束

加速度需要根据发动机动力和制动力的限制进行约束。该项为：

$$
a_{\text{low}} \leq u^T_k \begin{bmatrix} 1 \ 0 \end{bmatrix} \leq a_{\text{high}} \tag{24}
$$

其中 $a_{\text{high}} > 0$是发动机可提供的最大加速度， $a_{\text{low}} < 0$是制动器可提供的最大减速度。

2) 横摆角速度约束

偏航率需要根据方向盘转角限制进行约束：

$$
-\bar{s} \leq u^T_k \begin{bmatrix} 0 \ 1 \end{bmatrix} \leq \bar{s} \tag{25}
$$

其中 $\bar{s}$是最大偏航率，可通过最大方向盘转角并使用$\dot{\theta}= \frac{v}{L} \tan\delta$计算得到。

3) 边界约束

边界约束考虑纵向方向上的边界，可视为垂直于道路方向的线性约束。许多场景包含边界约束。如图4所示，停车标志场景可表示为交叉道路上的一个固定边界，而跟车场景可表示为前车后方的一个移动边界。

上述约束均为线性的，可直接遵循CILQR算法中的步骤 3处理。

4) 避障约束

避障约束考虑道路上的动态和静态障碍物。例如，如何安全地超车，以及如何避开路旁停放车辆等静态障碍物。由于约束是非凸的（可行状态空间位于凸形障碍物外部，例如用矩形表示的汽车），因此难以处理。此处我们忽略垂直轴，仅考虑平面问题。

本文中，我们将所有障碍物建模为椭圆，如图5所示。椭圆的长轴和短轴可根据障碍物形状（例如车辆的长和宽）及其速度进行设置。例如，对于一辆汽车，椭圆的长轴可设为
$$
a = l + v t_{\text{safe}} + s_{\text{safe}}
$$
短轴设为
$$
b = w + s_{\text{safe}}
$$
其中 $l$为车辆长度， $w$为车辆宽度， $v$为车辆速度，$t_{\text{safe}}$为安全车距， $s_{\text{safe}}$为安全裕度。

然后结合车辆的偏航角，可解析地写出该椭圆：

$$
(x - x_O)^T P(x - x_O) = 1 \tag{26}
$$

根据椭圆(26)的安全约束为$(x - x_O)^T P(x - x_O) > 1$。设 $x_k$为我们希望检查其安全性的点，并将安全裕度设为 $r$。则安全边界将是半径为 $r$的圆的圆心沿椭圆(26)边界运动所形成的轨迹。如图6所示，红色轨迹表示安全边界。该约束很难解析地表示，因此我们构建另一个椭圆来近似它。该椭圆的长轴为$a + r$，短轴为 $b + r$，表示为
$$
(x - x_O)^T P’(x - x_O) = 1
$$
图6中较大的黑色椭圆即为近似椭圆，它比真实的安全边界略小，但两者之间的差异可以忽略。

现在考虑新的椭圆和我们想要检查的点$x_k$，我们有以下约束：

$$
f_{x_k}(x_k) = 1 - (x_k - x_O)^T P’(x_k - x_O) < 0 \tag{27}
$$

这是一个非凸约束函数，需要遵循CILQR算法的步骤2。

由于车辆不能被看作一个简单的点，因此这里使用两个圆来表示汽车，如图7所示。其中一个圆位于后轴中心，另一个圆位于车辆前部的对称位置。在进行安全性检查时，需要检查两个圆的圆心，并以它们的半径作为安全裕度。当考虑前部圆心的安全性时，需要注意其位置是状态的非线性函数：
$$
p_{\text{front}} = [ p_x + \cos\theta \Delta l, p_y + \sin\theta \Delta l ]
$$
因此需要进行线性化过程。

我们选择椭圆来建模障碍物约束有几个原因。首先，由于椭圆具有可调节的长轴和短轴，因此能更好地表示移动障碍物。一些研究使用圆来表示障碍物，但在道路驾驶场景中并不合适，因为在这些场景中纵向运动通常主导横向运动。其次，椭圆是光滑的。有人可能会考虑使用矩形来更准确地逼近车辆形状，但矩形在其角点处会导致不连续导数。第三，由于高斯分布等同于椭圆，当考虑周围车辆的随机运动时，可以从当前的椭圆表示轻松扩展。

四、案例研究

研究了三种道路驾驶场景以展示CILQR的能力。仿真环境如图8所示。主车用蓝色框表示，周围车辆用红色框表示。每个周围车辆周围的黄色椭圆表示用于避障的安全裕度。蓝线表示主车应跟踪的参考轨迹。时间步信息通过颜色深度表示：更深的颜色代表较晚的时间步。

在以下案例中，假设在预览时域内周围车辆速度保持不变。

仿真的采样时间为$T_s = 0.25$s，预览时域为 $N = 40$。因此预览时间为$T = 10$s，足以进行长期规划。仿真在 Matlab中编写，并在配备2.6GHz Intel Core i7‐6600U的笔记本电脑上运行。需要注意的是，在以下案例中，计算可在0.2秒内完成，几乎是实时的。当算法采用更高效的编程语言（如C++）实现时，计算速度预计至少提高10倍，能够轻松满足实时性要求。

A. 案例1：静态障碍物避让

在这种情况下，道路上存在多个由椭圆表示的静态障碍物。主车需要安全地绕过这些障碍物。在图8中，规划轨迹由一系列颜色深度逐渐增加的蓝色方块表示，显示主车平滑地避开了障碍物。该案例的运行时间为 0.12秒。

B. 案例2：变道与跟车

变道是城市驾驶和高速公路驾驶中最常见的操作之一。通常目标车道中会存在前车，车辆在完成变道后需要进行自适应巡航控制（ACC）。执行变道时的一种危险情况是目标车道中存在与主车并行的车辆。我们构建了这种危险场景。如图9所示， $O^0_1$是位于$t = 0$的第一个障碍物， $O^T_1$是位于 $t = T$的第一个障碍物， $O^0_2$是位于 $t = 0$的第二个障碍物， $O^T_2$是位于 $t = T$的第二个障碍物。

在主车的规划轨迹中，车辆首先加速并绕过第一个障碍物以完成变道。变道完成后，车辆减速以对前车进行自适应巡航控制（ACC）。该场景的运行时间为 0.19秒。

C. 案例3：混合场景

在此场景中，主车的目标是在避让对向来车$O_2$的同时超越缓慢行驶的前车$O_1$，然后在位置 $x = 50$m处停止。通常情况下，下标表示障碍物的索引，上标表示时间。图10展示了主车的规划轨迹。从轨迹可以看出，主车首先加速，在遇到对向来车之前完成对前车的超车，随后减速并在停止线处停下。该案例的运行时间为 0.2s。

五、结论

本文提出了约束ILQR算法，用于处理ILQR的约束问题。该方法将约束进行线性化，通过障碍函数进行整形，并进一步二次化后嵌入ILQR的成本函数中。随后，采用CILQR算法求解了一般的道路行驶运动规划问题，其中考虑了到多项式参考线的距离以及障碍物的椭圆表示。案例研究表明，我们的算法在多种道路行驶场景中能够有效运行。