AL-iLQR 中的自车多圆拟合与碰撞

轨迹优化入门 · 交互式教程

为什么用多圆拟合自车

一辆车在真实世界里是矩形（或带倒角的长方形）。如果直接用矩形做碰撞检测，数学上要处理分离轴、角点、边的最近距离，每一步都带分支判断——这对基于梯度的优化器（iLQR 就是梯度 + Hessian 驱动的）非常不友好，因为梯度在角点附近不连续。

多圆拟合的想法是：用 N 个等半径的圆沿车身纵轴铺开，覆盖车身矩形。每个圆 vs 每个障碍物（也做圆近似）的碰撞判断就是两点距离平方——全程解析、光滑、二阶可导。代价是保守性：圆的并集比矩形稍大一点，但在规划阶段这是可接受的。

演示一：多圆如何铺满车身

调整圆的数量、车长、车宽，看圆如何在车体系下均匀铺开。圆半径固定为 $r = W/2$（这样圆刚好切到车身两侧），第一个圆和最后一个圆都要完全包在车身里。

圆的数量 N 3

车长 L (m) 4.8

车宽 W (m) 1.90

圆半径 r

0.95 m

覆盖纵向跨度

2.90 m

相邻圆间距

1.45 m

Code Mapping

VehicleCircleApproximation

vehicle_circle_approximation.cpp

const double radius = 0.5 * config_.width;
const double rear_limit = -config_.rear_axle_to_rear + radius;
const double front_limit =
    (config_.length - config_.rear_axle_to_rear) - radius;

circles_.reserve(config_.collision_circle_count);
if (config_.collision_circle_count == 1) {
  circles_.push_back(
      VehicleCircle{0.5 * (rear_limit + front_limit), 0.0, radius});
  return;
}

const double span = front_limit - rear_limit;
const double step =
    span / static_cast<double>(config_.collision_circle_count - 1);
for (int i = 0; i < config_.collision_circle_count; ++i) {
  circles_.push_back(
      VehicleCircle{rear_limit + step * i, 0.0, radius});
}

文件：src/autodrive/vehicle_circle_approximation.cpp。当前实现里所有圆都在车身纵轴上，所以 center_y_body = 0.0。

代码里的几何对应：圆心在车体系下 x 的取值范围是 $[-l_r + r,\ (L - l_r) - r]$，其中 $l_r$ 是后轴到车尾的距离。N 个圆在这段上均匀铺。N=1 时，落在范围中点，对应代码里 if (collision_circle_count == 1) 分支。

车体系 → 世界系

每个圆心在车体系下是 $(c_x^b, c_y^b)$。给定车辆位姿 $(x, y, \psi)$，圆心在世界系下是标准的 2D 旋转平移：

Formula

$$ p_i^w = \begin{bmatrix} x + \cos\psi \cdot c_x^b - \sin\psi \cdot c_y^b \\ y + \sin\psi \cdot c_x^b + \cos\psi \cdot c_y^b \end{bmatrix} $$

这是完整的 2D 刚体变换写法。当前仓库生成的覆盖圆都在中心线，因此实际代入时 $c_y^b = 0$。

Code Mapping

EvaluateWorldCenters()

vehicle_circle_approximation.cpp

const double x = state(0);
const double y = state(1);
const double yaw = state(2);
const double cos_yaw = std::cos(yaw);
const double sin_yaw = std::sin(yaw);

for (const auto& circle : circles_) {
  center(0) = x + cos_yaw * circle.center_x_body
            - sin_yaw * circle.center_y_body;
  center(1) = y + sin_yaw * circle.center_x_body
            + cos_yaw * circle.center_y_body;
}

先按 yaw 旋转车体系圆心，再叠加车辆的世界坐标平移。

这对应 EvaluateWorldCenters 里的代码。yaw 变化时，每个圆心绕后轴转；平移 $(x,y)$ 则整体搬。

当前仓库实现里覆盖圆都落在车体纵轴上，因此实际是 $c_y^b = 0$ 的特例；文中的写法保留了一般刚体变换形式，便于以后扩展到非中心线布圆。

演示二：碰撞约束 $g_i$ 实时可视化

把车辆放进世界，加一个圆形障碍物。拖动车辆或障碍物，看每个圆和障碍物的约束值 $g_i$ 如何变化：

Formula

$$ g_i(x) = (r_v + r_o)^2 - \|p_i^w - o\|^2 $$

$g_i \le 0$ 表示安全，$g_i > 0$ 表示第 $i$ 个覆盖圆与障碍物发生重叠。

Code Mapping

约束值实现

multi_circle_vehicle_obstacle_constraint.cpp

const double dx = centers[i](0) - obstacle_center_x_;
const double dy = centers[i](1) - obstacle_center_y_;
const double safe_distance =
    obstacle_radius_ + vehicle_geometry_.Circles()[i].radius;
values(i) =
    safe_distance * safe_distance - (dx * dx + dy * dy);

实现直接用平方距离，避开 sqrt，导数和 Hessian 近似都更稳定。

$g_i \le 0$ 表示安全，$g_i > 0$ 表示该圆已经和障碍物相交（需要惩罚）。

yaw ψ (度) 20

障碍物半径 0.80

圆的数量 N 3

拖动紫色车身或橙色障碍物。绿色圆表示 g ≤ 0（安全），红色表示 g > 0（碰撞）。

Code Mapping

Evaluate()

multi_circle_vehicle_obstacle_constraint.cpp

const auto centers = vehicle_geometry_.EvaluateWorldCenters(state);
Vector values(OutputDim());
for (int i = 0; i < OutputDim(); ++i) {
  const double dx = centers[i](0) - obstacle_center_x_;
  const double dy = centers[i](1) - obstacle_center_y_;
  const double safe_distance =
      obstacle_radius_ + vehicle_geometry_.Circles()[i].radius;
  values(i) =
      safe_distance * safe_distance - (dx * dx + dy * dy);
}
return values;

文件：src/autodrive/multi_circle_vehicle_obstacle_constraint.cpp。每个圆输出一个不等式，约束满足条件是 $g_i \le 0$。

几个要观察的细节：

每个圆都是独立的约束，所以一个时间步 $k$ 上有 $N_c \times N_o$ 个约束。把车拖到只有车头刚进障碍物时，前面的圆 $g$ 先变红——说明约束能精确定位"哪一部分"撞上了。
为什么用平方距离 $g = D^2 - \|\Delta\|^2$ 而不是距离 $\|\Delta\| - D$？因为 $\sqrt{\cdot}$ 在 $\|\Delta\| = 0$ 处不可导，平方形式的梯度是纯多项式，稳定又便宜。
$g$ 的量纲是"长度²"，所以惩罚系数 $\mu$ 的物理意义是"把 m² 的 $g$ 转成 cost 的单位"。

演示三：Jacobian 解析 vs 数值验证

iLQR 是牛顿类方法，每步要约束对 state 的 Jacobian $\nabla_x g_i$。把 $g_i$ 对 $(x, y, \psi)$ 求导：

Formula

$$ \frac{\partial g_i}{\partial x} = -2\Delta_x,\quad \frac{\partial g_i}{\partial y} = -2\Delta_y $$ $$ \frac{\partial g_i}{\partial \psi} = 2c_x^b(\Delta_x \sin\psi - \Delta_y \cos\psi) + 2c_y^b(\Delta_x \cos\psi + \Delta_y \sin\psi) $$

这是一般车体系偏移 $(c_x^b, c_y^b)$ 下的完整导数。当前仓库实现会落到中心线特例。

Code Mapping

当前代码中的退化形式

multi_circle_vehicle_obstacle_constraint.cpp

const double bx = circles[i].center_x_body;
jac(i, 0) = -2.0 * dx;
jac(i, 1) = -2.0 * dy;
jac(i, 2) = 2.0 * bx *
            (dx * sin_yaw - dy * cos_yaw);

因为当前圆心满足 center_y_body = 0，所以一般公式里的 $c_y^b$ 项不会出现在代码实现中。

上面是一般形式。对当前仓库的多圆车体，所有圆满足 $c_y^b = 0$，所以代码里的 yaw 导数退化成：

Current Implementation

$$ \frac{\partial g_i}{\partial \psi} = 2c_x^b(\Delta_x \sin\psi - \Delta_y \cos\psi) $$

这是当前仓库真正落地的 yaw 导数形式，因为所有覆盖圆都在车身纵轴上。

Code Mapping

yaw 列实现

multi_circle_vehicle_obstacle_constraint.cpp

const double bx = circles[i].center_x_body;
jac(i, 2) = 2.0 * bx *
            (dx * sin_yaw - dy * cos_yaw);

和上面的公式一一对应，其中 bx 就是文中的 $c_x^b$。

下面用解析 Jacobian vs 数值差分验证推导——两条曲线应该完全重合：

车辆 x (m) 1.5

车辆 y (m) 0.3

障碍物 x 3.0

障碍物 y 0.5

解析 ∂g/∂ψ 数值差分

最大误差（全 yaw 范围，前圆）

—

Code Mapping

JacobianState()

multi_circle_vehicle_obstacle_constraint.cpp

const double yaw = state(2);
const double sin_yaw = std::sin(yaw);
const double cos_yaw = std::cos(yaw);

const auto centers = vehicle_geometry_.EvaluateWorldCenters(state);
const auto& circles = vehicle_geometry_.Circles();
Matrix jac = Matrix::Zero(OutputDim(), 4);

for (int i = 0; i < OutputDim(); ++i) {
  const double dx = centers[i](0) - obstacle_center_x_;
  const double dy = centers[i](1) - obstacle_center_y_;
  const double bx = circles[i].center_x_body;
  jac(i, 0) = -2.0 * dx;
  jac(i, 1) = -2.0 * dy;
  jac(i, 2) = 2.0 * bx * (dx * sin_yaw - dy * cos_yaw);
}

当前实现没有 $c_y^b$ 项，不是推导漏掉了，而是因为 vehicle_circle_approximation 生成的圆心本来就在中心线上。

紫色实线（解析）和橙色虚线（数值）应该完全重合，最大误差是 $10^{-7}$ 量级（来自数值差分本身的截断误差）。如果 Jacobian 实现错了——比如 yaw 列正负号写反，或者以后扩展成 $c_y^b \ne 0$ 时忘了补上对应项——两条线立刻分开。生产代码里这样的对拍测试是必须的，因为 Jacobian 错误不会让程序崩溃，只会让 iLQR 慢慢不收敛，很难 debug。

演示四：增广拉格朗日函数

有了 $g_i \le 0$ 怎么优化？iLQR 本身只会做无约束的二阶优化。三种常见做法：

纯惩罚：$c = \tfrac{1}{2}\mu \max(0, g)^2$。简单，但要让约束严格满足，$\mu$ 必须趋于无穷，Hessian 会病态。
障碍函数（log barrier）：$c = -\tfrac{1}{t}\log(-g)$。要求轨迹始终严格可行，初始化苛刻。
增广拉格朗日（AL）：在惩罚基础上加一个乘子项，乘子会自己更新，让 $\mu$ 保持有限值就能精确满足约束。iLQR + AL = AL-iLQR。

AL 的每个约束 cost 项是（$\lambda \ge 0$ 是乘子，$\mu > 0$ 是惩罚）：

Formula

$$ \mathcal{L}(x) = \begin{cases} \lambda g + \tfrac{1}{2}\mu g^2, & \lambda + \mu g > 0 \\ -\lambda^2/(2\mu), & \lambda + \mu g \le 0 \end{cases} $$

对不等式约束，激活条件不是单纯看 $g > 0$，而是看 $\lambda + \mu g$ 是否进入正区间。

Code Mapping

AL 罚项本体

augmented_lagrangian_cost.cpp

const double projected =
    std::max(0.0, data.lambda(i) + data.mu(i) * values(i));
total += (projected * projected -
          data.lambda(i) * data.lambda(i)) /
         (2.0 * data.mu(i));

projected 就是激活量；当它为 0 时，对应公式的常数段 -lambda^2/(2mu)。

这个演示在画什么？　它不是在演"车辆怎么动"，而是在演一条不等式约束 $g \le 0$ 被 AL 包装之后，变成了什么样的 cost 曲线。

把横轴理解为约束值 $g$ 本身——不是位置、时间或 yaw，就是前面演示二里算出来的那个 $g_i = (r_v + r_o)^2 - \|\Delta\|^2$。约定：$g \le 0$ 安全，$g > 0$ 碰撞。

图上有三样东西：

紫线 $\mathcal{L}(g)$：AL 把这条约束"包装"成的 cost 值。优化器在最小化总 cost 时，自然会远离 $g > 0$ 区域。
绿线 $\partial\mathcal{L}/\partial g$：cost 对 $g$ 的导数（梯度因子）。iLQR backward pass 真正用的就是这个，再乘上 $\nabla_x g$ 得到加到 $l_x$ 里的推力。
红色背景：$g > 0$ 的违反区——进入这片区域就意味着碰撞。

两个旋钮的含义

$\lambda$（乘子）= "记忆"：表示这条约束在过去的外循环中被违反了多少。$\lambda$ 越大，即使当前 $g$ 还略小于 0（安全侧），cost 也会开始产生推力——提前推开，而不是撞到了才知道疼。

$\mu$（惩罚）= "硬度"：决定一旦约束被违反，cost 长得有多陡。$\mu$ 越大，紫线在 $g > 0$ 处越陡，优化器越不敢接近边界。

一句话记住：$\lambda$ 决定"多早开始推"，$\mu$ 决定"推得多狠"。

用碰撞约束举个例子

回忆演示二：$g_i = (r_v + r_o)^2 - \|\Delta\|^2$。假设 $r_v + r_o = 1.75\,\text{m}$，当前某个覆盖圆和障碍物的距离平方是 $\|\Delta\|^2$：

$g = -0.5$：距离平方比安全距离平方大 0.5 → 车圆离障碍物还有余量，约束满足。若 $\lambda = 0$，此时 AL 几乎不产生 cost。
$g = 0$：恰好贴着安全边界。只要 $\lambda > 0$，AL 已经在这里产生梯度，推动轨迹向远离障碍的方向走。
$g = 0.3$：碰撞了！距离平方比安全距离平方小 0.3。此时 $\tilde\lambda = \max(0, \lambda + \mu \cdot 0.3)$ 很大，cost 陡峭，iLQR 会拼命把这个时间步的轨迹拉回安全侧。

建议操作顺序

固定 $\mu = 5$，从 $\lambda = 0$ 慢慢拉大 → 观察绿线开始激活的位置向左移动。这说明"还没碰到约束，就开始推开"。
固定 $\lambda = 0$，从 $\mu$ 小拉到大 → 观察紫线在 $g > 0$ 区域变得越来越陡。这说明"违反约束的代价越来越大"。纯 $\lambda = 0$ 时其实就是纯二次惩罚，可以对比一下。
把 $\lambda$ 和 $\mu$ 都调大 → 感受两者叠加后的效果：约束还在安全侧就有推力，一旦越界则代价爆炸。

乘子 λ 2.0

惩罚 μ 5.0

L (cost) ∂L/∂g (梯度因子) 违反区 g>0

外循环后 λ 新值 (若 g=0.3)

—

若触发 penalty update 时 μ 新值 (ϕ=2)

—

Code Mapping

AL 罚项与导数

augmented_lagrangian_cost.cpp

const double projected =
    std::max(0.0, data.lambda(i) + data.mu(i) * values(i));
total += (projected * projected -
          data.lambda(i) * data.lambda(i)) /
         (2.0 * data.mu(i));

const Vector w = active.cwiseProduct(projected);
lx += Jx.transpose() * w;

const Vector mu_active = active.cwiseProduct(data.mu);
const Matrix Jx_scaled = mu_active.asDiagonal() * Jx;
lxx += Jx.transpose() * Jx_scaled;

文件：src/al/augmented_lagrangian_cost.cpp。代码里按 lambda + mu * g > 0 判断是否激活，不是简单按 g >= 0。

关键观察：

$\lambda = 0$ → 纯二次惩罚：此时只有 $g > 0$ 才有 cost，$g \le 0$ 完全无感。把 $\lambda$ 调大后，曲线整体被"抬起"并左移，梯度在 $g = -\lambda/\mu$ 处就开始激活——乘子会在约束还没碰边界前就推轨迹远离。这正是 AL 比纯惩罚更平滑、更容易收敛的核心。
绿线才是 iLQR 真正用的：梯度因子 $\partial\mathcal{L}/\partial g = \max(0, \lambda + \mu g) = \tilde\lambda$，再乘上约束对状态的导数 $\nabla_x g$（演示三里验证过的那个 Jacobian），就是加到 iLQR backward pass $l_x$ 里的贡献：$\nabla_x \mathcal{L} = \tilde\lambda \cdot \nabla_x g$。
Hessian 用 Gauss-Newton 近似是 $\mu \cdot \nabla_x g\, \nabla_x g^\top$（仅当 $\tilde\lambda > 0$），半正定，iLQR 回溯搜索不会崩。
外循环更新：每次 iLQR 内循环后，$\lambda \leftarrow \max(0, \lambda + \mu g)$——就是上面面板里显示的 "λ 新值"。而 $\mu \leftarrow \phi \mu$ 在当前仓库实现里是条件触发的，只有违反量下降不够才会放大 penalty。

整体流程：AL-iLQR 管线

新手常见坑

多圆数量 N 怎么选：轿车 3 个够了（前/中/后），长车或卡车 4~5 个。更多圆 = 更紧贴车身 = 更少保守，但 backward pass 里 $N_c \times N_o \times T$ 个约束的 Hessian 累加是主要开销。
安全距离 $d_{\text{safe}}$：把 $D = r_v + r_o$ 改成 $D = r_v + r_o + d_{\text{safe}}$，给 solver 留 0.1~0.3 m 余量。纯贴边的约束在 AL 外循环里数值抖动得厉害。
初值别让 $\mu$ 太大：建议 $\mu_0 = 1.0$，让 AL 外循环自己抬上去。一上来就 $\mu = 1000$，一旦进入约束激活区域，Hessian 的条件数会爆炸，iLQR 回溯搜索全是小步长。
动态障碍物：每个时间步 $k$ 的障碍物位置是预测值 $o_j(k)$，不是固定常量。单个 constraint 类固定障碍物的设计要扩展——或者在 Evaluate 时传入，或者让它接受 std::vector<Obstacle>。
Jacobian 的 yaw 列一定要单元测试：$\partial g / \partial \psi$ 的符号最容易写反；如果以后把圆心扩展到非中心线位置，还要记得补上 center_y_body 对应项。用中心差分 vs 解析，误差应该在 $10^{-6}$ 以下。
broad-phase 剪枝：离车辆 50 m 以外的障碍物没必要参与约束。加一个 AABB 预筛，大场景性能能好几倍。

AL-iLQR 入门 · 多圆拟合与碰撞约束

AL-iLQR 中的自车多圆拟合与碰撞

目录

为什么用多圆拟合自车

演示一：多圆如何铺满车身

车体系 → 世界系

演示二：碰撞约束 $g_i$ 实时可视化

演示三：Jacobian 解析 vs 数值验证

演示四：增广拉格朗日函数

两个旋钮的含义

用碰撞约束举个例子

建议操作顺序

整体流程：AL-iLQR 管线

新手常见坑