做 AL-iLQR 轨迹优化的人都知道，最磨人的往往不是优化器本身，而是碰撞约束怎么“喂”给求解器。之前几篇实践指南里我陆续写过了状态估计、障碍建模、代价函数设计，但到了真实车辆这个尺度，自车轮廓这个问题就完全绕不过去了：你不能把一个长 4.6 米、宽 1.9 米的矩形当质点用，也不能拿矩形去做距离约束然后指望 iLQR 端到端求出一个又光滑又可行的轨迹。

这次记录的是一个偏工程向的方案：用多个固定圆贴合自车轮廓，把碰撞约束转化成一组“圆到障碍物距离不得小于安全余量”的标量约束，再全部塞进增广拉格朗日 iLQR（AL-iLQR）框架里解。这个做法不是新潮玩意，但确实是我在实际项目里反复对比下来最稳定、最好调参、也最容易定位问题的一套组合。如果你已经在做 iLQR 相关的轨迹规划，或者正准备从质点约束往整车约束升级，这篇文章应该能让你少踩几个坑。

1. 自车多圆拟合：碰撞几何的拆解与选圆

1.1 为什么不能简单把车当成质点或矩形硬算

先讲清楚我为什么弃用“质点 + 障碍膨胀”的方案。质点的好处是约束容易写，一个点到所有障碍物的距离不小于某个阈值就完事。但真实车辆的横向宽度和前后悬摆在那儿，尤其在小半径转弯场景里，车尾会往外甩、车头会往内切，用质点模型去近似，实际执行时经常出现“优化认为很安全，实车却已经贴到障碍物”的情况。为了弥补这一点去暴力膨胀障碍物，又会让狭窄通道直接变不可行，得不偿失。

那直接用矩形做碰撞约束呢？理论上当然最精确，但工程上非常痛苦。点到矩形的距离函数本身是分片函数，最近点可能落在边上的任意位置，也可能落在顶点上，求导和线性化都需要做大量分类讨论。更关键的是，iLQR 在每一轮迭代里都会对约束做一阶或二阶近似，约束函数本身不够光滑，优化器就容易在矩形边缘附近震荡，甚至收敛到数值上不可用的轨迹。

多圆拟合的思路介于两者之间：用几个半径固定的圆去覆盖整车的矩形包络，把“自车碰到障碍物”等价成“这些圆中任何一个的圆心到障碍物边界的最短距离小于对应半径”。于是碰撞约束从“矩形到多边形的距离”变成了“若干个圆心到障碍边界距离”的并联约束。每个距离都可以用点到线段求交来算，求导结构简单、梯度连续性好，而且天然支持增量更新，非常适合反复迭代的优化器。

1.2 车辆轮廓的圆拟合实际构造方法

在做拟合之前，先把车的几何说清楚。我通常用一个简化包围矩形描述车辆：车头到前轴有一段前悬，前轴到后轴是轴距，后轴到车尾是后悬，再加上左右两侧的宽度。如果项目里没有精确数据，用一个典型尺寸就可以：总长约 4.7 米，总宽约 1.9 米，轴距约 2.8 米，前悬 0.9 米，后悬 1.0 米。

在这个矩形里“种”圆，最常见的是对称布置。圆的数量从 2 到 5 不等，数量越多越贴合矩形，但对应的约束数量也越多，优化求解负担相应变大。我的默认配置是 3 个圆：

• 后轴中心附近放一个半径较小的圆，覆盖车尾区域；
• 前轴中心附近放一个半径稍大的圆，覆盖车身中部；
• 车头前方再放一个圆，照顾车辆转向时的头部摆幅。

如果车辆轴距特别长，或者经常做大幅度转弯，我会在中间加第 4 个、第 5 个圆，保证转弯扫掠包络不被漏检。每个圆的圆心坐标和半径怎么定？我一般不是纯手调，而是把问题写成一个小的最小最大优化：

设定圆心坐标沿车身纵向偏移为变量，半径统一或分别独立，目标是最小化所有圆覆盖矩形后露出的最大“缺口”，同时要求圆不能超出矩形边缘太多，否则会把安全区域凭空扩大。实际工程里其实不需要追求严格最优，只要保证两件事：矩形四个角尽量被圆盖住，以及相邻圆之间没有明显缺口。这个用画图软件看一圈就能调出来，比上优化器更快。

1.3 圆数量与包络精度的取舍

圆不是越多越好。我试过 7 个圆的方案，包络确实严丝合缝，但优化时间几乎翻倍，而且多余的约束在宽度方向上有很强的冗余，导致增广拉格朗日乘子之间相互牵扯，最后收敛速度反而变慢。个人经验是：普通城区低速泊车场景用 2 到 3 个圆就够；结构化道路变道场景加 1 个车头圆；需要原地旋转或频繁大转向的场景才建议 5 个圆。

这里有一个直观的类比：车辆转弯时车身扫过的面积并不是一个简单矩形，更像一个向内收缩、向外扩张的“扫掠带”。多圆拟合并不能保证每一瞬间都完全贴合扫掠包络，但可以在关键位置兜住车辆外轮廓。你只需要保证“圆覆盖区域始终包含真实车辆包络”，碰撞约束的安全性就成立了。

我常用的三类配置如下表，方便直接抄：

需要强调一点：拟合圆的半径应该比车辆半宽略大，多出来的部分作为基础安全余量。这个余量和后面讲的碰撞安全距离是两个概念，前者是几何近似误差，后者是规划器对控制偏差和感知误差的预留，两者不能混为一谈。

2. 碰撞约束的 AL-iLQR 建模

2.1 iLQR 与增广拉格朗日的基本气质

先说 iLQR 的核心逻辑。iLQR 会围绕当前轨迹将系统动力学线性化、将代价函数二次化，然后通过逆向传播得到一条局部最优的反馈控制序列，再正向仿真更新状态轨迹。这个流程反复迭代，直到轨迹收敛。它的优势是计算结构基本上是矩阵乘法和回代，实时性好；短板是处理复杂不等式约束比较绕。

躲障碍物本质就是一堆不等式约束：位置必须保持在可行空间里。直接把这些约束硬加进 LQR 里，会让原本清爽的线性二次结构被破坏掉。常见的替代思路有三种：纯惩罚法、约束投影法、增广拉格朗日法。纯惩罚法简单，但罚系数大了容易奇葩、小了约束无法严格满足；投影法在约束边界复杂时容易来回弹跳；增广拉格朗日法是在原来的目标函数上加一项“带乘子的二次惩罚”，然后用外层循环更新乘子。

AL-iLQR 把增广拉格朗日思想装进 iLQR 迭代里，等于把不等式约束拆成两部分处理：内层 iLQR 只解一个无约束或只带简单约束的最优控制问题，外层负责更新乘子和罚系数。这样做的好处是，既保留了 iLQR 的高效数值结构，又能让最终解在满足约束的方向上不断被“拽”回去。实际项目里，AL 方法对一个已经接近可行的初始轨迹几乎就是“临门一脚”，非常舒服。

2.2 圆到障碍物的距离约束怎么写

对于每个圆，我定义它有一个编号 c，圆心在世界坐标系中的位置是 p_c，半径为 r_c。给定一个障碍物，我先把障碍物拆成若干条线段（矩形就拆成四条边），然后计算圆心点到每一条线段的最短距离，取所有线段里的最小值，作为“该圆到该障碍物”的几何距离 d_c。

碰撞约束就是：

d_c >= r_c + safe_gap

其中 safe_gap 是额外留出的安全距离。实际工程中，我会把 safe_gap 加进圆半径里，也就是直接约束 d_c >= R_c，其中 R_c = r_c + safe_gap。这样代码里只需要维护一个“等效半径”，约束形式更统一。

为什么用“点到线段”而不是“点到多边形”？因为线段距离的梯度特别干净：最近点要么落在线段内部，此时梯度方向就是圆心到垂足的方向；要么落在线段的两个端点之一，此时梯度方向就是圆心到端点的方向。这个逻辑写满也只是几个 if 分类，不会出现高阶非光滑项，传给 iLQR 的雅可比矩阵非常友好。

2.3 多圆并联约束与平滑聚合

一个自车可能有 3 个圆，一个圆周围可能有 5 个障碍物。那么理论上的碰撞约束应该是“所有圆到所有障碍物的距离中的最小值大于安全值”。但如果直接把取最小值这个操作放进约束公式里，约束函数在多个圆之间会发生切换，产生尖点，也就是不可导点。

为了避免这种切换，我通常用两种方式处理：

第一种是“分离约束”，也就是不取最小值，而是把每个圆对每个障碍物的约束都单独写成一行不等式约束。这样约束数量会多一些，但每个约束都是光滑的，乘子更新也方便。缺点是当障碍物很多时，约束矩阵会膨胀。

第二种是“平滑聚合”，用类似 LogSumExp 的平滑最小函数去近似所有距离中的最小值。这样做约束数量少，但平滑参数不能调得太大，否则会出现“看起来约束满足，实际已经穿透”的假象。

我的建议是：地图中障碍物数量少时用第一种，把每个圆每块障碍的约束都显式写出来，方便排障；稠密交通流里用第二种，减少约束数量换计算速度。如果用的是平滑聚合，平滑系数的取值要在“梯度连续”和“约束保守性”之间找平衡。我一般把平滑系数设为 0.05 到 0.2 之间，具体取决于车辆尺寸。

3. 实操实现：从几何到乘子的完整闭环

3.1 状态、控制与车辆动力学定义

我的 AL-iLQR 里状态量取 x = [X, Y, θ, v, δ]ᵀ，控制量取 u = [a, ω]ᵀ。X、Y 是车体参考点（通常取后轴中心）在世界坐标系下的位置，θ 是航向角，v 是纵向速度，δ 是前轮转角。a 是纵向加速度，ω 是前轮转角速率。

离散时间模型我常用一个符合低速泊车和城区巡航场景的简化自行车模型：

X_{k+1} = X_k + v_k * cos(θ_k) * dt Y_{k+1} = Y_k + v_k * sin(θ_k) * dt θ_{k+1} = θ_k + v_k / L_wb * tan(δ_k) * dt v_{k+1} = v_k + a_k * dt δ_{k+1} = δ_k + ω_k * dt

其中 L_wb 是轴距。注意这里车的参考点在后轴中心，所以圆心的位置要从状态量里推导出来。比如第 i 个圆在后轴中心坐标系下的纵向偏移是 l_i、横向偏移是 b_i，那么它的世界坐标就是：

p_i_x = X + l_i * cos(θ) - b_i * sin(θ) p_i_y = Y + l_i * sin(θ) + b_i * cos(θ)

圆心位置是状态量的函数，所以距离约束对状态的梯度非常自然，直接走链式法则就能得到。这也是多圆拟合方便落地的核心原因：几何上只是加一个坐标变换，代码上完全不需要改 iLQR 的主流程。

3.2 距离约束的雅可比求解细节

要让 iLQR 正确利用碰撞约束，需要拿到每个约束对状态量的一阶梯度。对某个圆来说，先求圆心到障碍线段的最短距离 d，并记录最近点 q 在线段上的参数位置。

如果最近点在线段内部，那么从圆心到垂足的方向就是约束梯度方向；如果最近点是某个端点，约束梯度方向就是从圆心到该端点的方向。这个方向向量记为 n_hat，然后对圆心位置 p 求导是 n_hat，再对状态 x 求导就是 ∂p/∂x 与 n_hat 的复合。

∂d/∂x = n_hatᵀ * ∂p/∂x

∂p/∂x 里面无非是 cos、sin 和偏移量，这个推导我建议用手推一遍，不要依赖自动微分。原因有一个： iLQR 每次迭代都要把约束对状态求导，如果能手写出解析梯度，代码执行效率会高一个量级。自动微分虽然方便，但在这种强实时任务里没必要把性能让出去。

还有一个细节：如果圆心到线段的最短距离出现在线段延长线上，距离公式仍然成立，但梯度方向会有微小的误差。工程上我会做一个小的截断处理，把参数 s 限制在 [0, 1] 范围内，再从端点取梯度。这相当于在距离函数里引入了一个很小的近似的噪声，但对优化收敛几乎没有影响。

3.3 AL 内外循环结构和乘子更新

AL-iLQR 的实现我习惯分两层循环。外层循环更新增广拉格朗日的乘子 λ 和惩罚系数 ρ，内层循环用 iLQR 求解带惩罚项的增广目标。

内层 iLQR 每一步迭代时，除了标准的轨迹代价和终端代价，还要加上：

J_aug = J_original + Σ λ_i * g_i + Σ 0.5 * ρ * max(0, g_i)^2

其中 g_i = R_i - d_i，表示第 i 个约束的违反量。max(0, g_i) 只对违反约束的部分做惩罚，满足约束的部分不叠加额外代价，这样比较自然。

外层循环每轮结束以后，用当前违反量更新乘子：

λ_i = max(0, λ_i + ρ * max(0, g_i))

同时如果违反量下降缓慢，把 ρ 乘一个 1.5 到 2.0 的倍率；如果违反量已经小于阈值，就维持 ρ 不变，防止过大的 ρ 导致轨迹刚硬化。

我遇到过有人把 ρ 一开始就设成 1000，然后发现轨迹被惩罚项压得笔直，宁可绕大远路也不敢贴障碍物，路线质量非常差。更好的做法是把 ρ 从 10 或者 50 起步，让轨迹先自由优化出一个大方向，再逐步用乘子和罚系数把轨迹拉进可行域。这个顺序非常重要，顺序反了很容易让整个轨迹陷入局部极小。

3.4 典型调参配置和收敛判定

下面这组参数是我在某个模拟项目里用了很久的配置，可以作为初始值参考，但不同场景一定需要微调：

收敛判定不要只看总代价下降。我会同时看三件事：约束最大违反量是否小于 0.02 米、轨迹终点的位置误差是否小于给定阈值、相邻两次迭代的控制量变化是否足够小。三者同时满足才算收敛。

有一点要提醒：AL 方法允许中间轨迹违反约束，这是正常行为。你看到第 5 轮迭代时轨迹还穿模，别急着加罚系数，先看乘子是否在增长。乘子增长说明约束正在被纳入优化目标，这是好现象；如果乘子不动但轨迹在抖，那多半是梯度计算有 bug。

4. 避坑与排查记录

4.1 圆包络本身的安全余量严重不足

我最早做多圆拟合时，为了让包络更贴合矩形，把圆半径压得很紧，结果仿真中经常出现“圆没碰到障碍物，但车辆角点已经刮到”的情况。原因是车辆真实包络和圆包络之间总会有拟合误差，哪怕只有几厘米，在车辆做大转角时也会被放大。

后来我改成每个圆在拟合半径基础上再加一个 inflation 量。具体做法是：把圆的等效半径设定为拟合半径 + 0.08 米，并且把这个值作为约束的一部分直接带入计算。这样做的代价是通行宽度会被吃掉一点点，但换来的是碰撞零容忍场景下的稳定安全。要记住：多圆拟合是一种保守近似，宁可多占一点安全距离，也不要赌拟合误差的方向。

4.2 距离函数光滑化过度导致约束穿透

用平滑聚合时，有一个非常典型的坑：平滑系数越大，梯度越平缓，优化器越容易收敛到局部解，但平滑后的“最小距离”会比真实最小距离偏大，于是车辆实际上已经离障碍物很近了，约束却显示还没违反。

我的实测经历是，当平滑系数超过 0.5 时，轨迹在狭缝中穿过的概率明显上升，因为约束值被“平均”掉了。建议在需要严格防碰撞的场景里，不要用平滑聚合，而是使用显式的分离约束。如果一定要用，把平滑系数控制在 0.1 左右，并在约束计算时额外补偿一个 0.05 米的保守偏移量。

4.3 乘子初始化不当导致轨迹全程避险过度

AL 乘子是一个很敏感的东西。如果一开始就把乘子设成很大的值，优化器会认为任何接近障碍物的路径都是高代价的，最后可能给出一个绕远路甚至倒退行驶的怪异轨迹。我习惯把乘子初始化为 0，让第一轮 iLQR 纯粹靠原有代价函数的“惯性”把轨迹拉向目标，后面再用惩罚项修正方向。

还有一个常见的错误是乘子更新太激进。λ 更新的幅度如果过大，轨迹会在某一轮突然被推离障碍物很远，然后在下一轮因为追赶目标又靠近障碍物，形成震荡。解决方式是限制 λ 单步增幅，比如最多只允许在上一轮基础上增加 2 倍，超过这个范围就保持不变，等内层迭代自然适应。

4.4 可视化调试的必做动作

这可能是所有调试技巧里最重要的一条：把每个圆的圆心、半径、距离线段和最近点全部画出来，叠加在轨迹动画上调试。不要只在结果里看 vehicle footprint in a top-down view。我通常在优化循环的每次外层迭代里，都输出一下该轮轨迹上各个控制点的“圆到障碍最近距离”曲线。如果最近距离曲线在某个时间点突然掉到接近零再弹起来，那大概率是货车转弯扫掠包络漏了中间的那个圆，而不是算法崩溃。

这么画的另一个好处是能快速定位乘子的作用对象。你会清楚看到乘子到底在“拉”哪个圆的哪个方向，而不是对着一个聚合成的最小距离值干瞪眼。AL 方法的最难调教之处就在这种逐条约束的交互上，一个收敛性的局部最小值往往能通过再加一个约束、改一个乘子初值就解决掉。

4.5 多障碍物稠密场景下的约束裁剪

如果地图里有 20 个障碍物、自车 5 个圆，那么理论上要生成 100 条分离约束。iLQR 每次迭代里 100 条约束都求导、都算乘子，不仅慢，还有大量约束是无效的，因为该圆和某个障碍物之间距离非常远，根本不构成威胁。

实际项目中我会做一个候选约束筛选：先计算每个圆的圆心到每个障碍物中心的距离，只保留“圆心距离 < 圆半径 + 障碍外接半径 + 剪裁阈值”的约束组合，其他的直接丢弃。剪裁阈值一般取 1.0 米，这样既保证安全，又把单轮约束数量压缩到了一个很稳定的水平。剪裁本身不会破坏求解正确性，因为被剪掉的约束在真实距离上离安全边界还很远，乘子变化也几乎可以忽略。

4.6 初始轨迹对 AL-iLQR 的成败影响

最后需要强调的一点，也是我反复跟团队说的：AL-iLQR 不是万能黑盒，它对初始轨迹的合理性非常敏感。如果你拿一条完全横穿障碍物的直线作为起始轨迹，增广拉格朗日极大概率会收敛到某个贴着障碍物绕行的怪异路径，而不是把车优雅地拉回车道。

一个稳妥的做法是先用粗糙的全局路径规划器生成一条符合拓扑的参考路径，再把它离散化成 iLQR 的初始轨迹。AL 乘子在大方向不对时会帮优化器找到局部修正，但不要指望它能做全局拓扑变换。多圆碰撞约束在这套流程里承担的是“最后一公里”的精细化修正：保证路径不刮擦、不留安全冗余，而不是负责从零找出一条路来。

我个人实际操作中最受益的一点是：把碰撞约束的复杂度从矩形降到多圆以后，很多以前躲不开的调试死角都变得清晰可见了。圆与圆的梯度是独立的，乘子也是独立的，哪个圆在哪个方向上有问题，一眼就能定位。这种“可解释性”是这套方案最大的额外价值。等到后期想更进一步，可以考虑把障碍物也拆成圆或者胶囊体，让距离计算变成圆与圆的解析距离，那样梯度更干净，但碰撞精度会略微下降，适合与本文方案做个对比验证，看哪种更贴合你的实车表现。