通用混合整数自动驾驶规划

自动驾驶最优行为规划：一种通用的混合整数形式

克莱门斯·埃斯特尔1∗，托比亚斯·凯斯勒1∗和阿洛伊斯·克诺尔2
摘要—混合整数二次规划(MIQP)已被认为是一种适用于 以低运行时间求解行为规划问题最优解的合适方法。该方法同 时优化逻辑约束和连续方程。然而，此前仅针对直路进行了模 型公式化，忽略了在交叉路口转弯等常见情况，这使得该模型 至今无法在现实中应用。基于三重积分器模型公式，我们计算 车辆朝向，并以析取方式对其进行建模。这使我们能够建立线 性约束，以考虑非完整性和避障需求。这些约束为近似方法， 我们引入了相应的理论基础。我们在两个基准场景中展示了其 适用性，并通过使用非线性优化求解相同模型来验证可行性。
这一新模型将使研究人员能够利用MIQP的优势，例如逻辑约 束或全局最优性。

一、引言

自动驾驶行为规划组件的任务是计算一个安全通过 交通并达到目标状态的计划。该计划通常被定义为一系 列未来的状态或航点，然后传递给轨迹跟踪控制器。如 果这些点由于未考虑车辆的非完整性约束而无法被跟踪， 则存在安全风险，并最终可能导致碰撞。

基于优化的方法引入了运动学模型，并在给定的规 划时间范围内进行传播，通常通过构建约束来保证可行 性与安全性，同时设计成本函数以考虑舒适性及其他期 望因素。尽管序列二次规划（SQP）等局部连续优化方 法已在实际中应用，但混合整数规划（MIP）在最优行 为规划中具有多项优势。首先，MIP能够得到最优解， 而SQP仅能保证获得局部最优；其次，MIP允许引入 逻辑和整数约束，而这些约束通常会导致连续求解器出 现数值问题。然而，MIP无法使用非线性车辆模型（如 自行车模型），因为这会使相应的优化问题难以求解。
以往的研究采用二阶或三阶积分器来缓解这一问题[2, 3]。
然而，所提出的方法只能用于一小部分场景，即直道，并 且在其他任何环境（环岛、交叉路口）中都会失效，甚至

∗这些作者对本工作贡献相同。 1fortiss有限公司，巴伐利亚自由 州研究院，德国慕尼黑2慕尼黑工业大学，机器人、人工智能与实时系 统讲席，德国慕尼黑

碰撞形状Ego 参考轨迹 优化轨迹 碰撞形状障碍物
图1：我们方法的概述。从任意环境形状和不可跟踪的参考轨 迹出发，我们计算出最优运动。通过对车辆在不同朝向下的碰 撞形状采用适应性约束进行过近似，我们的模型具有全局有效 性，并能避开任意障碍物。

在低速障碍物规避过程中，由于模型公式化的有效范围 有限。这引出了一个问题：如何在如图1所示的混合整数 规划框架中，构建一个具有正确车辆动力学的通用全局 有效线性化模型。
我们提出一种适用于车辆完整的方向范围的模型，该模型 具有
•线性过近似碰撞约束，
•车辆的线性非完整约束，
•通过线性最小二乘拟合计算所有模型参数的方法论，以及
•对模型的可行性的详细研究。

II.相关工作

Ziegler等[1]提出了一种最优控制问题，其中应 用了表I中所示的代价泛函和约束。他们使用三重积分 器作为车辆模型，同时通过限制曲率来考虑非完整性。
贝尔塔‐奔驰驾驶展示了在正确约束下三重积分器模型的 适用性。他们的方法产生了一个非线性优化问题，该问 题使用SQP求解但仅能找到局部最优。受此启发，机动 规划的方法[4, 5]专注于在初步步骤中找到正确的机动。
然而，这些方法通常依赖于工作空间的几何划分，因此 可扩展性较差，并且无法扩展以考虑任何交互式或协同 规划。
Nilssonet al.[6]基于线性双积分器模型提出了两 个用于纵向和横向控制的二次规划问题（QP）。为了考 虑非完整性约束，作者使用了耦合横向和纵向速度的线 性不等式约束。然而，该方法仅在偏航角较小时有效， 在交叉路口会产生不可行驶轨迹。

表I：模型方法的比较。我们使用1表示线性泛函，2表示二次泛函，n表示所有其他非线性泛函。对应的1、2和n表示未实现（且无 法实现）的泛函。我们对假设为理想的代价泛函进行比较，主要依据[1]。 j表示代价项， κ表示曲率， v表示速度， a表示加速度。

自动驾驶最优行为规划：一种通用的混合整数形式	Ziegler等[1]	古特亚尔等[7]	Nilsson等[6]	Qian等人[3]	Frese和Beyerer[2]
问题表述	SQP	纵向和横向分别使用二次规划纵向和横向分别使用二次规划	MIQP	MILP
模型	三重积分器	Frenet自行车模型双重积分器	双重积分器	双重积分器	双重积分器
参考系	笛卡尔，固定	Frenet，沿街	笛卡尔，固定	笛卡尔，固定	笛卡尔，固定
非完整性约束	κ, n	κ, 1	vx vy是耦合的，1	vx vy是耦合的，1	alat, 1
加速度约束|a|	2	1	n	n	近似，1
碰撞形状	圆盘	圆盘	道路对齐矩形	道路对齐矩形	道路对齐矩形
碰撞检查到	所有事物	所有事物	道路对齐矩形	道路对齐凸多边形	非凸道路多边形
碰撞约束	n	1	1	1	1
j速度	n	2	n	n	n
j加速度	2	2	2	2	1，2
jjerk	2	2	2	2请参见[12]	2
j偏航率	n	2使用 κ ̇1	n	n	n
j参考	n	2	n	n	n
有效性	任何道路/方向	任何道路/方向	直路，与道路方向对齐	多智能体	no no no 是的，参见[12] yes

例如环岛，由于未考虑道路曲率。
Gutjahret al.[7]基于自行车模型，在弗雷内坐标系中 引入了两个纵向和横向的二次规划。该模型在静态环境中行驶
时表现出良好的效果。与Ziegleret al. [1],类似，该方法依 赖于动作选择，如Esterleet al.[8]所示。然而，将所有障碍 物转换到局部坐标的计算代价较高，随着障碍物数量的增加， 这一开销会超过快速二次规划所带来的优势。
Milleret al.[9]基于[7]开展研究，并采用混合整 数二次规划（MIQP）在局部道路方向坐标下构建了两 个连续的纵向和横向规划，方法类似于[6],。然而，该 方法无法考虑基于模型的预测或多智能体规划。此外， 将纵向和横向控制分离会导致次优解，并限制解空间。
Frese和Beyerer[2]提出了一种基于双重积分器的 混合整数线性规划模型，利用其生成最优解的能力。但 该方法仅能处理线性约束和目标函数。因此，我们根据 线性泛函1、二次2以及非线性泛函n对表I中的模型进行 了分析。通过限制横向加速度的近似值来保证非完整性 约束，但这仅适用于较小的偏航角，并会产生大量无效 解[10]。碰撞检测采用任意道路多边形，并通过凸多边 形的析取碰撞检测进行建模。他们还提出了使用基于矩 形的车辆近似对连续状态进行碰撞检测，每种变体都会 引入大量无效轨迹[10]。
Qianet al. [3]将双重积分器应用于MIQP。与 Nilssonet al.[6],类似，他们通过限制横向速度来建模 非完整性，但这仅适用于直道且车辆沿道路方向行驶的 情况。即使在避障时，偏航角也可能超过20度，这可能 导致优化轨迹中出现真实车辆无法执行的弯折。因此， 该方法仅限于直道和直线行驶，而在

交叉路口处转弯则无法实现。由于MIQP的二次代价函数， 只有当参考信号为零（例如加速度）时，才可能对纵向或 横向值（如加速度）的差异进行表达。因此，无法表示对 绝对速度的偏差。Burger和Lauer[12]通过将状态空间扩 展到多个智能体，将Qianetal.[3]的工作扩展到了协同环 境。然而，该方法的局限性也被继承了下来。
Kessler和Knoll[13] 提出了一种混合整数规划， 用于为一组智能体选择最优协同行为。他们不依赖优化 中的模型，而是使用运动基元，使其适用于任意环境。
然而，当采用细粒度离散化时，优化问题会变得很大。
总之，目前尚无方法能将混合整数规划（全局解、 逻辑约束）的优势应用于通用环境。

III.基于区域的线性化方法

继Qianet al. [3],之后，我们将车辆建模为三阶质点
系统，其状态包括位置 p x(t)、 p y(t)，速度 v y(t)、 v y(t)，
以及加速度 a x(t)、 a y(t)。两个方向上的急动度 j x(t)和
j y(t)作为模型的输入。由于我们旨在将车辆模型表述为线 性约束，因此必须消除所有非线性部分。接下来，我们将介 绍如何保证模型在所有方向下的有效性，并执行碰撞检测。

A.方向的离散化与析取建模

尽管车辆的方向 θ 不属于状态空间，但在优化问 题中，我们需要它来在笛卡尔坐标系下进行充分的碰撞 检测。我们假设车辆具有完美的抓地力，因此忽略车辆
和轮胎的滑移。该方向可通过 θ= atan(v y /v x )计算得 出。然而，此方程以及三角运算均为非线性。
sin(θ)= sin(atan(v y /v x )) (1a) cos(θ)= cos(atan(v y /v x )) (1b)

区域1 区域 N 4 图2：通过两条线(0, 0)−(α,β)和(0, 0) −(γ,δ)构建区域 i，如公式(2)所 述)

需要计算前轴位置。因此，为了建立碰撞约束，我们 需要对公式(1)进行线性化。为了让我们的模型在方向
θ ∈[0, 2π]下有效，我们通过在(vx, vy) 平面引入区域
来对方向进行离散化，参见图2。这些区域由从原点出发
的两条线段之间的区域定义。因此，对于每个位于区域 i
内的(vx, vy) 点，以下不等式成立，其中线参数依赖于 区域：
αvy ≥βvx (2a)
γvy ≤δvx (2b)
完成这一步后，我们将随后建立在不同参数的每个 区域中均有效的模型方程。这些方程列于表II中。
出于驾驶舒适性的考虑，大多数运动规划器会限制 车辆行驶方向的最大纵向和横向加速度、减速度以及急 动度。根据车辆行驶方向的这些期望值，我们在全局 x, y坐标系下计算相应区域的边界。该过程通过将原始的 纵向和横向限制随车辆朝向进行旋转实现。方向角采用 相应区域的平均角度，从而确保在行驶方向上满足原始 限制的绝对值和方向要求。

表II：本文中使用的拟合或预处理参数。区域依赖性用 r表示。

参数描述
α r x区域的值 r区域下边界线
β r y区域的值 r区域下边界线
γ r x区域的值 r上区域边界
δ r y区域的值 r上区域边界
P r cos 在 v x 、 v y 中线性的多项式，上界逼近cos(θ)
P r cos 在 v x 、 v y 中线性的多项式，下界逼近cos(θ)
P r sin 在 v x 、 v y 中线性的多项式，上界逼近sin(θ)
P r sin 在 v x 、 v y 中线性的多项式，下界逼近sin( θ )
P r κ 在 v x 、 v y 中线性的多项式，下界逼近 κ
P r κ 在 v x 、 v y 中线性的多项式，上界逼近 κ
u r x , u r x 方向 x的急动度下限和上限
u r y , u r y 方向 y的急动度下限和上限
a r x , a r x 方向 x的加速度下限和上限
a r y , a r y 方向 y的加速度下限和上限
ρ r 区域 r在当前场景中是允许的

图3：示例非线性函数及其相应的分段线性上和下边界。)

B.碰撞形状的过近似

如果方向已知，一种常见的近似车辆形状的策略是 使用三个半径为r的圆形，分别对应后轴、中间位置和 前轴，类似于[1],，因为这种方法可以高效地对任意多 边形进行碰撞检测。我们的目标是采用线性车辆模型， 因此真实的（高度非线性）方向是未知的。由于我们只 有近似的方向，并且不希望低估任何碰撞风险，因此我 们选择计算该方向正弦和余弦的上界与下界。图3展示了
这一思路。结合车辆的轴距l，我们随后计算前轴 x和 y
位置的上界和下界。
f x ∶= p x + l cos(θ) (3a) f x ∶= p x + l cos(θ) (3b) f y ∶= p y + l sin(θ) (3c) f y ∶= p y + l sin(θ) (3d)
将 f x 、 f x 与 f y 、 f y 进行排列组合，可得到前轴的四 个圆，这些圆对真实前轴圆形成过近似，如图4所示。目 前，我们选择不建模车辆的中点，因为这会增加模型的 复杂性。然而，类似的方法也可应用于中轴。接下来， 我们介绍两种获取正弦和余弦边界的方法。

1)常数近似：

我们提出使用每个区域的最大和最小 朝向对正弦进行常数近似。
sin(θ)= ̃ max[sin(atan(δ r ,γ r )), sin(atan(β r ,α r ))] (4a) sin(θ)= ̃ min[sin(atan(δ r ,γ r )), sin(atan(β r ,α r ))]. (4b)

[r ad ] 图5：sin(θ)= sin(atan(vy/vx))关于 vx和 vy的图像。该函数 明显是高度非线性的，但可以通过分段线性方式进行近似。)

余弦相应地进行计算。随着区域数量的增加，此类 近似值的误差将减小。请注意，只有当一个区域不跨越 多个象限时，此结论才成立，因为正弦和余弦在每个象 限内才是单调函数。

2)速度相关近似：

为了保持约束的线性性，只能计
算状态变量的线性组合。我们选择通过一个依赖于区域 r
的一阶多项式来对sin(θ)项进行上界近似，该多项式包
含三个参数 p sin(θ)= ̃p00+ p10vx+ p01vy ∶= Pr sin(vx, vy) (5)
类似地，为正弦 Pr sin函数的下界以及余弦 Pr cos和 Pr cos
的边界拟合此类线性多项式。我们在第四节‐A部分中介 绍了计算这些参数 p的方法论。该近似将导致前轴位 置依赖于相应的速度项。然而，如果方向可以解析地计 算，则不会出现这种情况，并且在低速时会产生较大误 差。

C.非完整建模

之前的混合整数二次规划公式[3, 11, 12]通过在 x和
y v y ∈
[vx (θmin) v x (θmax)] θ min θ max
方向上限
制加速度，并通过tan、tan将速度耦合，其中和是该模型的有 效方向范围，来近似非完整约束。然而，解耦的加速度边界无法产 生非完整约束行为。Ziegleret al.[1] 使用曲率进行计算
κ= v x a y −v y a x
3√v 2 x + v 2 y
(6)
并建立边界约束 κ ∈[κmin ,κ max] 。然而，公式(6)是高 度非线性的，因此曲率约束无法表示为MIQP的线性约束。
为了获得依赖于 a x 、 a y 的约束，我们通过公式(6)对 κ max 、
κ min 进行变换以
κ max
v x 3√v 2 x + v 2 y + v y
v x
a x ⪖ a y (7a) κ min
v x 3√v 2 x + v 2 y + v y
v x
a x ⪕ a y . (7b)

[rad] sin(θ)−sinLB[rad] −20 −10 0 10 20 −20 −10 0 10 20 vx[m/s] v y [m /s ] cosUB−cos(θ)[rad] −20 −10 0 10 20 vx[m/s] cos(θ)−cosLB[rad] 4 · 10−2 6 · 10−2 8 · 10−2 0.1 0.12 0.14 0.16 图6：使用32个区域对上界 UB和下界 LB三角函数进行分段线性 拟合的误差。)

然后，我们可以应用第三节‐A部分中描述的分区域线性化概念，以获得线性约束，并拟合两条线性多项式来分别限定y轴上方 Pr κ和下方 Pr κ的曲率边界，如第IV‐B节所示。

IV.拟合方法

在本节中，我们将简要描述如何拟合III‐B.2和III‐C中的 参数。所有拟合均基于区域进行，并得到公式(5)形式的多项式。

A.前轴位置的拟合

在第三节‐B中，我们阐述了需要对依赖于 vx和 vy的 三角函数公式(1a)和公式(1b)进行线性化的原因。正弦和 余弦函数均具有高度非线性，在图5中我们展示了正弦函 数。我们将问题表述为：通过带线性约束的线性最小二 乘问题，寻找关于 vx和 vy的二维非线性函数的分段上界。
图6显示了拟合所得到的误差。对于正弦和余弦函数的上 界和下界，方向误差始终低于0.16弧度。由于
sin(vx = 0, v y → ±0)= ±∞ (8a)
cos(vx → ±0, v y = 0)= ±∞, (8b)
在接近原点处，我们得到最大的误差。对于较高的 速度，由于对非线性函数的近似值更好，误差显著更小。
表III显示了第一象限内不同方向下前轴位置误差与 常数近似（记为const.）的比较。我们观察到上界和下 界始终符号相反，这意味着实际轴位置始终位于两者之 间。这表明对前轴的过近似具有有效性。如预期，随着 区域数量的增加，误差变得更小，因为我们使用更小的 线性段来近似非线性函数。总体而言，速度相关近似的 误差小于常数近似的误差。对于速度 ⪅ 0.1m/s，常数近 似产生的误差更小，其原因由公式(8)说明。在两种近似 方法之间实现最优过渡策略是未来工作的一部分。

V.将规划问题表述为混合整数二次规划问题

我们将规划问题建模为混合整数二次规划公式，且不 限制模型的有效性范围。车辆运动模型被表述为离散线性 约束。通过使用二元变量，我们确保车辆的无碰撞性和正 确的非完整运动。一个二次目标函数使解保持在参考值附 近。
随后，我们使用表IV中引入的决策变量命名规则。
下标 ref 表示相应的参考。我们优化从 k 2 到 k N 、以Δt
为增量的离散时间范围。用 K表示时间间隔[k1 ,…, k N]。
所有决策变量均以车辆在 k 1 处的当前状态进行初始化。
我们将速度限制在拟合所得的最小值 v和最大值 v范围内，
因为我们的近似方法仅在此范围内有效。下标
9
表示 同时适用于 x和 y方向的相应项。接下来，只要不会引 起歧义，我们将使用此符号表示以提高方程的简洁性。

表IV：本文中使用的决策变量，包含离散时间依赖性 k、区域 依赖性 r、环境依赖性 λ以及障碍物依赖性 ℴ。

变量描述	范围
px(k), py(k)车辆后轴中心位置	free
vx(k), vy(k)车辆在 x, y方向的速度	[v, v]
ax(k), ay(k)车辆在 x, y方向的加速度	[a, a]
ux(k), uy(k)车辆在 x, y方向的急动度	[u, u]
fx(k), fy(k) 车辆前轴的上界 中心位置	free
fx(k), fy(k)车辆前轴的下界 中心位置	free
ρ(k, r) r区域，车辆在时间 k所处的区域	二进制
Ψ(k)不允许区域变更	二进制
e(k,λ)车辆不在环境中
子多边形 λ在时间 t	二进制
op(k,ℴ)车辆不与障碍物ℴ发生碰撞
在时间 t位于参考点 p	二进制

A.将参考跟踪制定为目标函数

作为成本函数 J，我们选择了位置和速度距离参考 的加权和。对于加速度和急动度，我们旨在最小化其平 方值以避免变化。合适的代价项 q可平衡解。
J= ∑
k∈K (qp(px(k) − px,ref(k))2+ qp(py(k) − py,ref(k))2 + qv(vx(k) − vx,ref(k))2+ qv(vy(k) − vy,ref(k))2
+ qaax(k)2+ qaay(k)2+ quux(k)2+ quuy(k)2)
(10)
为了构建一个混合整数二次规划问题，目标函数必须是 平方项或线性项之和。因此，我们不能在目标函数中使
用绝对速度（或加速度），因为当|v| = √(vx2+ vy2)时，
项( |v|2 − |vref|2)2不是二次的。类似地，对角速度施加成 本将导致非二次项。此外，由于我们在模型中不计算到 物体的距离，因此无法在成本函数中使用这些距离项。
请注意，通过这种目标函数的制定方式，我们跟踪的是 参考轨迹而非参考路径。

B.将车辆模型制定为约束条件

车辆动力学定义为：
⎡ ⎢ ⎢ ⎣
p
9 (ki+1) v
9 (ki+1) a
9 (ki+1)
⎤ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎣
1 Δt Δt 2 /2 0 1 Δt 0 0 1
⎤ ⎥ ⎥ ⎦
⎡ ⎢ ⎢ ⎣
p
9 (ki) v
9 (ki) a
9 (ki)
⎤ ⎥ ⎥ ⎦ + ⎡ ⎢ ⎢ ⎣
Δt 3 /6 Δt 2 /2 Δt
⎤ ⎥ ⎥ ⎦ u
9 (ki)
∀k i ∈[k1 ,…, k N−1] (11)
通过该线性模型，正确的非完整运动以及正确的加速度 和转向角限制仅在小范围的参考方向附近有效。我们通 过引入有效性区域来克服这一特性，详见第三节‐A部分。
覆盖360度完整方向的一组区域表示为 ℛ。在下文中， 我们用上标 r表示区域相关参数。所有区域相关参数汇 总于表II。
我们设置二进制决策变量 ρ( k, r )，用于定义车辆在时间 k 处于哪个区域 r，依据是划分该区域的两条直线。

∑
r∈ℛ
ρ(k, r)= 1 ∀k ∈ K (12)
由于随着区域数量的增加，模型的评估时间也会随
之增加，因此我们先验地将优化限制为仅使用一组允许
的区域，并相应地预先计算参数 ρr 。那些在规划范围 内本来就无法到达的区域被视为不允许的区域。为了实 现逻辑约束，我们采用引入大常数 M来切换不等式的常 用技术。现在经常使用的术语 M(1 −ρ(k, r))的直观解 释是：“该方程仅在区域 r在时间步 k处于激活状态时 才生效”。激活区域由以下一组约束条件确定：
αrvy(k) ≥βrvx(k) − M(1 −ρ(k, r)) (13a) γrvy(k) ≤δrvx(k)+ M(1 −ρ(k, r)) (13b) ∀k ∈ K, ∀r ∈ℛ, if ρr(r)= 1
由 ρr标记为不可能的区域不得被选择。通过此实现， 该模型公式化适用于所有场景。
ρ(k, r)= 0 ∀k ∈ K, ∀r ∈ℛ, if ρr= 0 (14)
我们对加速度和急动度施加区域相关限制，以确保始终
满足正确的绝对可能加速度和急动度。由于加速度 a 9和急
动度u 9是在全局坐标系中定义的，因此这些限制会随区域 旋转。在方程(15)中，我们仅列出加速度的约束，急动度 的表述类似。注意，速度自然地由方程(13)正确地施加了 限制。
a 9(k) ≤ ar 9
+ M(1 −ρ(k, r)) (15a) a 9(k) ≥ ar 9
−M(1 −ρ(k, r)) (15b) ∀k ∈ K, ∀r ∈ℛ, if ρ r = 1

C.将非完整约束建模为约束

为了确保车辆的正确非完整运动，我们使用每个区 域的最大可用曲率来限制横向加速度，如第三节‐C所述。
我们拟合了依赖于 v x 、 v y 和区域 r的下界和上界线性近
似多项式 P r κ 和 P r κ 。以下约束模型化了公式(7)中所述 的对曲率 κ的边界不等式。
a y (k) − β r +δ r
α r +γ r
a x (k) ≤
P r κ (v x (k), v y (k))+ M(1 −ρ(k, r))+ MΨ(k) (16a) a y (k) − β r +δ r
α r +γ r
a x (k) ≥
P r κ (v x (k), v y (k))+ M(1 −ρ(k, r))+ MΨ(k) (16b) ∀k ∈ K, ∀r ∈ ℛ, if ρ r = 1

在极低的车辆速度下，过紧的曲率约束会限制加速 度，使得除零之外的其他加速度无法实现。相反，过松 的曲率约束将违反非完整性。因此，我们引入最小速度 限制。如果|vx|或|vy|低于该限制，则不允许区域变化， 我们通过将二进制辅助变量 Ψ设置为真来表示这一点。
ρ(ki, r) −ρ(ki−1, r) ≤ 1 −Ψ(ki) (17a) ρ(ki, r) −ρ(ki−1, r) ≥ −1+ Ψ(ki) (17b)
∀ki ∈[k2,…, kN], ∀r ∈ℛ

D.将前轴位置近似作为约束

在第三节‐B中，我们介绍了如何近似车辆前轴位置 的上下界的概念。利用这一思想，我们构建了约束条件， 以对前轴进行（过近似的）碰撞检测，而不是计算实际 车辆形状与障碍物或环境之间的交集或距离。我们计算 相对于当前区域 r 的真实但未知的前轴位置(fx, fy) 的
上下界 f 9、 f 9，其中 l 表示车辆的轴距。这里仅给出 上界的方程，下界约束的表述类似。
M(ρ(k, r)−1) ≤ f x(k) − p x(k) − lPr cos(vx(k), vy(k))(18a) M(1−ρ(k, r)) ≥ f x(k) − p x(k) − lPr cos( v x(k), vy(k))(18b) M(ρ(k, r)−1) ≤ f y(k) − p y(k) − lP r sin(vx(k), vy(k))(18c) M(1−ρ(k, r)) ≥ f y(k) − p y(k) − lP r sin(vx(k), vy(k))(18d) ∀k i ∈ K, ∀r ∈ℛ, if ρ r (r)= 1

E.约束限制模型保持在 Road

本节介绍如何强制车辆保持在建模为任意的、可能 非凸闭合多边形 Λ[2]的环境中。该环境通过减去碰撞 圆的半径进行收缩，参见图7。非凸的环境多边形被分割 成多个凸子多边形 λ。我们强制车辆至少位于其中一个 凸子多边形内。这些子多边形由一组线段 l表示，每条
线段连接两个点 a l和 b l。采用此策略后，多边形到多 边形的碰撞检测简化为点对多边形检查。

我们约束后轴位置 p9以及前轴位置的上下界，即 四个点(fx, fy)、(fx, fy)、(fx, fy)和(fx, fy)，使其位于环 境多边形内。每组约束均以类似方式表述，式(19)给出了 点(px, py)的方程。决策变量 e表示在时间k，这五个点均 不与环境子多边形 λ发生碰撞。其中， l9分别表示
bl 9 − al 9对于 x和 y的情况。
lx(py(k) − al y) − ly(px(k) − al x) ≤ −Me(k,λ) (19) ∀k ∈ K, ∀l ∈λ, ∀λ ∈Λ
为确保所有车辆点至少位于一个环境多边形内，我们设定
λ∑∈Λ e(k,λ) ≤ |Λ| −1 ∀k ∈ K, (20)
其中|Λ|de环境。 notes the number of convex sub-e

F.将避障制定为约束条件

我们强制车辆不与任意数量的静态或动态凸多边形 障碍物 O发生碰撞。如第V‐E节所述，非凸形状的障碍 物被分割为多个凸多边形。障碍物通过碰撞圆的半径进 行膨胀，参见图7。
l再次表示障碍物集合 O中某个障碍物的一条线段， 其起点为 al ，终点为 bl。我们要求车辆后轴位置 p9以 及前轴边界的四种排列均处于无碰撞状态。与假设为恒 定不变的环境不同，障碍物多边形的位置和形状可能随 时间变化，但保持其多边形拓扑结构不变。通过决策变
量 o 9，我们表示这五个点中是否没有任何一个点与障碍 物 ℴ发生碰撞。公式(21)给出了对点(px, py)施加约束的
不等式 op。代表前轴碰撞形状近似值的四个点 f 9，每
个点都通过另外四组类似的决策变量 o
9
和相应的不等式 集合予以考虑。
l x(py(k) − a l y) −l y(px(k) − a l x) ≤ Mo p(k, l) (21) ∀k ∈ K, ∀l ∈ℴ, ∀ℴ ∈ O
用|ℴ|表示子多边形中的线段数量，我们约束五个点 中的每一个点都不位于障碍物内部。
∑
l∈ℴ
o p(k, l) ≤ |ℴ| −1 ∀k ∈ K, ∀ℴ ∈ O (22)

G.优化mization Problem

收集上述所有约束后，最终的优化问题可以表述为
minimize(10) subject to(11),(12),(13),(14),(15),(16), (17),(18),(19),(20),(21),(22)
该公式是一个标准的MIQP模型，可使用现成求解器求解。
为了更快地获得优化解，我们使用常数对加速度 a
9
和急
动度 u
9
进行约束
0 5 10 15 0
5
10
15
x[m]
y[ m ]
参考
Ours
SQP（自行车）
SQP（nl. κ constr.）
0 5 10 15 0
5
10
15
x[m]
y[ m ]
| |后轴|
| —|—|
| |前近轴似矩形|
| |前近轴似矩形|
| |前近轴似矩形|
0 0.5 1 1.5 2 2.5 3 3.5 4 −0.1
0
0.1
t[s]
κ [1 /m ]
参考
Ours
限制

值 a、 a 和 u、 u 分别。我们选择这些值作为区域相
关限制 ar 9、 ar 9的最小值和最大值
and ur 9 , ur 9
respectively.

VI.评估

现在，我们将在两个不同的场景中评估我们的模型。
在第VI‐A节中，我们证明了优化结果是一条适用于非完整 车辆模型的可行驶轨迹。第VI‐B节展示了在避障场景中的 性能表现。我们仅对参考位置跟踪应用代价项。我们使用 了一个包含32个区域并采用速度依赖的前轴近似值的模型。

A.保持非完整性

为了展示我们所提出的保证非完整运动的约束的有效 性，我们优化了两条不同的参考轨迹，这两条轨迹均形成 一个圆形以完成90度转弯。尽管理想情况下应仅生成具有 有效曲率的参考轨迹，但本示例清楚地表明我们的模型保 留了非完整性。这一特性对于障碍物规避同样至关重要。
作为比较的基线算法，我们实现了两个基于SQP的优化模 型：一个使用标准自行车模型，另一个使用与我们的 MIQP模型相同的三重积分器，但采用非线性曲率约束，遵 循公式(6)。

图8展示了我们的车辆模型能够遵循的转弯半径的结 果。我们的优化生成的轨迹紧密跟随参考轨迹。然而， 图9中的第二个参考轨迹所建模的转弯半径过小（参考轨 迹的曲率超出了限制）。如预期所示，我们的模型未跟 随该参考轨迹，而是生成了一条保持在曲率边界内的轨 迹。由于我们在每个区域的均值处拟合曲率近似多边形 （见第IV‐B节），在区域边界处可能会略微超出曲率边 界。这可以通过使用安全裕度轻松缓解。

B.道路边界内的动态障碍物避让

第二个示例考虑了一条双车道道路，其中自车以 30km/ℎ的速度行驶，并接近一辆以 10km/ℎ行驶的慢速 车辆。对向车道有以 30km/ℎ行驶的对向交通。参考轨迹 表示右车道的中心线，以参考速度行驶。我们获得了这 两个交通参与者确定性预测结果，并将其作为动态障碍 物纳入优化中。图10显示，优化器能够找到一条超车前 方红色车辆并返回右车道以避让绿色对向车辆的轨迹。
我们观察到 y方向上的加速度保持在加速度限制范围内。
该加速度比公式(16)给出的曲率引起的加速度限制要小 得多，这是合理的，因为在中高速度下的直线场景中， 横向运动受到车辆惯性的约束，而非非完整约束。

VII.结论与未来工作

我们提出了一种用于行为规划问题的新型混合整数 形式。我们展示了优化结果在任意道路曲率和车辆任意 朝向下正确的非完整运动特性。通过使用碰撞形状的线 性过近似，我们确保MIQP公式不会导致任何碰撞。这 为我们提供了一种求解方法，能够获得运动学上有效、 全局最优且避障的解，而无需任何形式的随机性。这一 特性对于验证与认证而言无疑是一个有利特征。未来， 我们将能够在真实道路驾驶场景中应用MIQP行为规划 模型，并结合本研究所的研究车辆[14]进行实践。未来 工作将扩展该框架至多智能体情况下的协同规划，并研 究逻辑约束在交通规则表述中的优势。