1. 为什么在多圆拟合之前我要先放弃“把车当成质点”先聊一个看起来基础、实际上很容易埋雷的问题在 AL-iLQR 这类轨迹优化框架里自车到底应该怎么描述很多新手跑通第一版碰撞避免时整天盯着障碍物做避让对自车反而特别敷衍——直接取后轴中心当质点或者画一个包络矩形就开算。这样做在开阔场景里确实“看起来能用”路径是绕开了障碍物可一旦进入窄通道、泊车、会车这类空间紧张的操作矩形的角点、后悬外摆、车头扫过的圆弧都会变成隐性碰撞源。你明明在回放里看到保险杠贴着墙过去了但算法给出的“安全距离”却虚高原因只有一个自车几何根本没进优化模型或者进得太粗糙。AL-iLQR 的全称是 Augmented Lagrangian Iterative Linear Quadratic Regulator增广拉格朗日迭代线性二次调节。它本质上是把带约束的非线性轨迹优化问题转成一串无约束子问题来迭代。上一篇文章里我们用单圆包络、甚至用轴点半径做膨胀障碍物那一套思路在空旷环境完全够用但自车尺寸一旦变大单圆包络就荒唐了要么空余体积大到没法通过窄门要么圆半径逼近车长一半优化器会认为四面八方都是“禁入区”连正常并线都变得畏手畏脚。所以我更愿意把这一篇定位成“几何建模篇”在把自车放进 AL-iLQR 之前先用一组互不依赖的圆形覆盖住车辆真实外轮廓再用这些圆的距离约束去表达碰撞条件。多圆拟合不是一个新鲜点子但在实际项目里圆的个数、圆心坐标、半径怎么分配、碰撞距离怎么平滑、惩罚项怎么挂接这些细节如果只按论文默认去写往往能稳定复现问题却不能稳定复现成功。这一篇的目标读者是已经跑通基础版 iLQR、想往真车约束上靠的开发者。你会看到为什么矩形和单圆都不够好多圆拟合的参数怎么定以及在 AL-iLQR 框架里如何把“圆到障碍物的最近距离”接进代价函数。2. 单圆保守、矩形难微自车包络的两种失败打开方式2.1 单圆包络的最大问题不是半径是无效空间先做个简单的几何推导。假设一辆车长 4.7 米宽 1.9 米对角顶点距离约 5.07 米。如果用单圆包络整个矩形轮廓圆心取在车身几何中心圆半径必须大于 2.54 米才能罩住四个角。这意味着在车辆横向上单个圆比车身两侧宽出接近 1.6 米。在停车场场景里这个 1.6 米就是灾难。一个 2.5 米宽的车位两排车中间通道也就 5~6 米你的“安全圆”直接宣判通道不可用。更隐蔽的问题是单圆在所有方向上的安全距离都是一样的车头方向其实只需要 0.9 米的前悬余量但圆模型却要求前后左右统一预留 2.54 米。结果是横向过于保守、纵向又未必真的安全——因为圆与矩形之间在车头中段方向存在不小的“无效真空区”。有人会说那把障碍物膨胀半径加大一点不就行了抱歉膨胀障碍物解决的是“自车离障碍物多远才算安全”它不解决“自车到底占了多少空间”。车辆包络和障碍物膨胀是两套几何混用之后只会变成调参玄学。2.2 矩形包络进入优化器的烦恼距离函数不光滑既然单圆不行矩形总行了吧可矩形进入优化器之后问题出在“最近距离”的梯度上。在 AL-iLQR 里每一轮正向滚动和反向传播都要计算约束函数对状态的偏导。如果自车是矩形车辆到某个点的最近距离函数是由点在矩形的哪个区域决定的点在矩形外侧正向投影时距离取点到边的垂直距离点在矩形角点方向时距离取到角点的欧氏距离。这两种计算方式的切换边界恰好是梯度不连续点。优化器在那个区域里来回震荡反向传播给出的搜索方向会突然跳变表现出来就是轨迹在某个障碍物附近抖来抖去惩罚项明明在涨控制量却一直在左右横摆。你可能会说用 smooth-min 或者软距离函数不就行了可以平滑但矩形包络的另一个问题来了车是带航向角的刚体矩形在自车坐标系里随航向旋转做外部场景解析时每一个角点坐标都得经过旋转矩阵变换雅可比推导一下子就长了五六行。圆没有这个烦恼圆对航向角天然不变。2.3 多圆拟合的思路用一组小圆把“安全边界”填得尽量接近真实外轮廓于是多圆拟合的意义就清楚了它追求的既不是单圆那种“全局覆盖”也不是矩形那种“精确边界”而是一种在优化可微性、空间保守性之间取平衡的折中。做法上就是在自车坐标系里摆若干个圆这些圆的并集要能完全覆盖车辆真实外轮廓。碰撞约束不再针对车身本身计算而是针对每一个圆计算“该圆圆心到最近障碍物的距离是否大于该圆半径”。这带来的直接好处是几何检查变得很快点对点的距离总比点对多边形边界的光滑距离便宜而多圆只是把这个点对点距离重复算几遍。而且每个圆的梯度计算相互独立反向传播时只需要叠加各自贡献完全不影响 AL-iLQR 在时间轴上本来该有的稀疏结构。我见过不少实现方案圆的个数从 4 个到 12 个不等。但我的经验是先不要追求“圆的并集等于车辆轮廓”那会变成过度拟合。实际上多圆的并集略微大于车身是没有问题的只要不出现“横向过剩”太离谱就行。真正要控制的是“最大外凸余量”也就是圆往外超过车身侧沿的距离这个值尽量在 0.1~0.2 米以内。3. 自车多圆拟合的参数化圆心放哪、半径给多大、覆盖怎么验证3.1 一组可复用的基础参数为了讲清楚我用一辆典型中型车的尺寸来做例子。记住这些数不是拍脑袋而是可以换成你自己的车参数再算一遍车长 4.70 m车宽 1.90 m轴距 2.86 m前悬 0.95 m后悬 0.89 m。把车辆放在平面坐标系里以后轴中心为原点x 轴指向车头y 轴指向左侧。那么车头最前端在 x 轴距/2 前悬 1.43 0.95 2.38 m车尾最末端在 x -(1.43 0.89) -2.32 m车身边缘在 y ±0.95 m。一个我实测下来性价比很高的方案是 5 圆布置圆编号纵向位置 x (m)横向位置 y (m)半径 r (m)主要覆盖区域前圆1.850.000.55车头中段左前圆0.550.750.45左前角、左侧前段右前圆0.55-0.750.45右前角、右侧前段后圆-1.600.000.55车尾中段、后悬区域中圆-0.200.000.55车身中段、侧向余量对照这辆车的四个角点坐标前保险杠左右角约在 (2.38, ±0.95)后保险杠左右角约在 (-2.32, ±0.95)。前圆圆心 (1.85, 0) 半径为 0.55最远覆盖到 x2.40、y 到 ±0.55显然盖不住前保险杠的左右角点。算法逻辑上左右前圆就是来补这两个角的左前圆圆心 (0.55, 0.75) 半径 0.45它的最左最上边界能到 x1.00、y1.20……等等这还不够靠近车头角点。所以我要再强调上面这组表只是“第一版草稿”必须画图验证。验证方法很简单在自车轮廓上均匀撒点判断每个点是否落在至少一个圆内。如果某些点漏了就调整对应圆的圆心或加大半径。这里给出我实际上会收敛到的 5 圆布置经验值圆编号纵向 x (m)横向 y (m)半径 r (m)前中圆1.7000.78左前圆0.750.800.55右前圆0.75-0.800.55后中圆-1.4500.78中圆-0.3500.55这样前中圆的半径 0.78最前边界到 2.48超过车头 2.38能罩住车头中段左右前圆共同补掉左右角点区域。验证撒点之后最大漏出量可以压到 5 厘米以内。3.2 覆盖验证要量化不能靠肉眼肉眼在屏幕上过关不代表数学上安全。我建议把“覆盖率”写成一个离线检查脚本以 0.02 m 间距在车身矩形内撒点对每个点判断是否在任一圆内统计“未覆盖点”占整个轮廓边界带的比例和最大外凸距离。边界带我取的是车身外轮廓向内 0.2 米范围内的区域因为对碰撞约束而言圆并集往里缩一点问题不大只影响“很贴车身的障碍物判定”但边界带不能有缝。一个常见误区是把圆心放在纯纵向一条线上导致车身左右两侧的侧向余量只有中圆兜底。中圆半径 0.55 时圆心到车身侧沿 0.95 米的横向间隙是 0.4 米这相当于说在 x-0.35 这一横截面上车身侧向还剩 0.4 米真空。这不是不能接受但停车场景里如果侧方障碍物离车侧 0.3 米圆判定就会漏报。所以遇到狭窄场景我会把 5 圆升级成 7 圆在左右两侧各加一个侧圆专门收掉车侧中段的横向余量。3.3 圆的半径要不要带余量这是很容易被忽略的小细节。我建议业务上设两层半径真实半径用于碰撞判定安全余量在障碍物膨胀侧去加。不要直接在圆半径上加 0.15 米否则所有圆的梯度表达式里会出现一个隐形的常数偏移看起来无害但会让“车身与障碍物之间明明还有 0.1 米距离”的工况被误判为碰撞。安全余量的推荐做法是在障碍物多边形原始边界上统一做内向缓冲。这个缓冲与自车圆半径无关逻辑也更清晰——障碍物膨胀量决定了你希望车身离边界多远自车圆只负责表达车身轮廓。4. AL-iLQR 里的碰撞约束从点到圆的距离函数到增广拉格朗日松弛4.1 约束函数的构造圆距离的最小值还不能直接用当自车被拆成 K 个圆之后“碰撞约束”表达成什么假设第 k 个圆圆心位置为 \( p_k \)半径为 \( r_k \)某个障碍物表示为多边形。约束函数最常见的一种是\[ c_k(p_k) \text{dist}(p_k, \mathcal{O}) - r_k \]其中 \( \text{dist}(p_k, \mathcal{O}) \) 是圆心到障碍物多边形的最小距离。约束要求 \( c_k \geq 0 \)。问题在于“多边形最小距离”这个函数本身是分段光滑的。圆心距多边形某条边较远时最近距离由点到边的投影决定圆心靠近顶点时最近距离由欧氏距离决定。直接在 ILQR 里挂这样的非光滑约束反向梯度会产生类矩形包络一样的振荡。我的做法是在实际代码里用一个光滑替代先求出圆心到多边形所有边、所有顶点的候选距离然后用 log-sum-exp 做近似最小区取。\[ \tilde{d} -\lambda \log\left(\sum_i \exp(-d_i / \lambda)\right) \]其中 \( d_i \) 是圆心到第 i 条候选原语的距离滑动参数 \( \lambda \) 设 0.1 左右。这个式子从来不会说出比真实最小距离更小的值它只会轻微偏大偏大方向对碰撞判定是安全的。实际测试里\( \lambda0.1 \) 时误差在毫米级优化特性却平滑很多。4.2 一次性约束太多圆反向传播会互相打架很多人把 K 个圆的约束直接并排写进 AL 增广目标里\[ J J_{\text{nominal}} \sum_{t} \sum_{k} \left[ \mu_{t,k} c_{t,k} \frac{\rho}{2} c_{t,k}^2 \right] \]然后在反向传播时时间步 t 上的状态梯度是 K 个约束项各自对自车状态求导的叠加。看起来合理但实际调试中非常容易出问题前圆约束和后圆约束在同一时间步上会给出方向相反的梯度。尤其车辆带航向角时前圆的最近障碍物在左前方后圆的最近障碍物在右后方两个约束同时想把自己往外推轻则收敛变慢重则轨迹整体出现不自然的横摆。一个更稳的工程做法是在同一时间步先计算各圆的约束值然后只选择约束值最小的那个圆也就是“当前离碰撞最近的圆”进入代价。这不只是一个硬截断而是一个 min 操作虽然 min 也不光滑但可以用上面说过的 log-sum-exp 对 K 个约束值再做一次平滑取小。这样单个时间步只有一个主导圆在起作用反向传播的梯度保持一致方向求解器稳定性提升明显。4.3 增广拉格朗日为什么要用而不是单纯罚函数如果读者以前接触过软约束轨迹优化大概率会想直接上大惩罚不行吗行但效果很差。固定惩罚太大会让 iLQR 过度关注约束甚至驱动控制量剧烈变化前进后退来回试探固定惩罚太小约束又根本不满足。AL 的核心思路是引入拉格朗日乘子 \( \lambda \)迭代核心是每轮求解完无约束子问题后检查约束残差并把乘子沿残差方向更新。对比固定惩罚AL 的好处是同样一个约束最终收敛时惩罚权重不需要无限大而是保持一个适中量级乘子承担了“纠正偏差”的职责。在碰撞场景下这意味着轨迹不会因为权重过大而变得僵硬该贴的走廊还是能贴过去只是把车身姿态调整得更好。在 AL-iLQR 的上下文中每次外层迭代 一次完整的 iLQR 求解过程。求解完毕后\[ \lambda_{t,k} \leftarrow \lambda_{t,k} \rho \, c_{t,k} \]然后固定 \( \rho \)继续下一轮外层迭代。\( \rho \) 只有在约束迟迟不收敛时才逐步翻倍。4.4 实际上怎么挂进 iLQR 的代价函数以离散时间轨迹优化为例设状态有位置、航向角和速度视你动力学模型而定。在第 t 个时间步iLQR 的局部二次目标只是标称代价与约束代价之和的二次近似。因为碰撞约束直接依赖位置状态而位置状态一般只是从控制量积分来的它的雅可比算起来也不难正向仿真从初始状态逐级积分得到完整状态轨迹。约束计算对每个时间步用平滑后的圆约束函数计算 c_t 和偏导 \( \partial c_t / \partial x_t \)。反向传播从末端倒着算 Q 函数、控制律与值函数。更新控制量用新控制律做一次前馈加反馈更新。外层更新乘子。关键细节在于乘子更新不能太激进。我用的是对每一时间步先计算“这个时间步有没有约束激活”如果当前乘子加惩罚量之后约束仍然紧张才更新如果约束远未被激活乘子退回 0。否则所有时间步的乘子都会缓慢增长最后导致背景里到处都有微小的推挤力轨迹质量反而下降。5. 调参实战权重、惩罚系数、平滑参数怎么配才稳5.1 惩罚系数的启动值与倍增策略我常用的初始配置是拉格朗日乘子初值 \( \lambda0 \)惩罚系数 \( \rho \) 从 100 开始每两层外部迭代若约束残差没有降到设定阈值\( \rho \) 乘以 4但如果约束残差连续两轮下降就维持 \( \rho \) 不变。为什么从 100 而不是更大的 10000 开始因为初期 iLQR 还在调整全局轨迹约束残差大是正常的惩罚系数过大会立刻压制其他优化目标导致轨迹偏离标称路径太远后续纠正回来更难。而 100 这个量级对于“露出边界 10 厘米”这种残差产生的梯度足够让轨迹往安全区挪动又不会把整个趋势带偏。5.2 平滑参数 \( \lambda \) 与碰撞判定的关系前面 4.1 节里用了 log-sum-exp 来平滑多边形的最近距离。这里的 \( \lambda \) 大小直接影响安全边界\( \lambda0.01 \) 时平滑距离几乎等于真实距离但梯度在顶点附近恢复陡峭\( \lambda0.5 \) 时距离函数很顺滑但会高估真实距离最多 5~10 厘米。我实测比较舒服的值是 0.1 到 0.2。0.1 的高估误差在毫米到厘米之间而它对近顶点区域的梯度连续性好很多。如果你发现自己轨迹离障碍物边界总是贴着 3 厘米在走别急着怀疑碰撞函数先看 \( \lambda \) 是不是给大了。5.3 状态初始化对收敛的影响多圆拟合让约束得了位置但位置初值离可行域远近也很大程度决定收敛时间。我见过最典型的失败案例是初始状态路径直接穿过障碍物第一轮 iLQR 的线性化点离障碍物太近反向传播算出巨大的梯度控制量一次性变化过大轨迹直接蹦到障碍物另一侧然后来回穿越。这时候不是算法问题是初始化问题。我的处理是分两步走先用一个不含碰撞约束的纯标称 iLQR 跑一段参考轨迹保证它从起点到终点的拓扑方向正确再把这段参考轨迹作为碰撞约束版 AL-iLQR 的初始化。这样即使初始轨迹会发生碰撞它的“几何趋势”已经是合理的增广拉格朗日只需要微调局部姿态。6. 实测中会遇到的怪问题与排查思路6.1 轨迹明明没碰到障碍物碰撞约束却一直激活排查思路先输出每个圆的约束值找到到底是哪个时间步、哪个圆在报警。多数时候你会发现是前悬或后悬区域的圆没有跟车身真实外轮廓对齐尤其低速大转角工况下前保险杠外侧会扫出比直线行驶更大的包络弧而拟合圆只按直线几何做了导致圆被侧向挤到障碍物里。解决方案不是加大半径而是按“最大转角时的车辆包络”重新撒点拟合。低速挪车场景常用前轮最大转角约 35 度模拟打满方向时的车头/车尾外侧轨迹再把圆的半径微调到覆盖动态包络。6.2 优化在障碍物旁来回摆动这通常不是约束本身的问题而是控制量变化权重设太小。碰撞约束想贴边但是优化器允许控制量高频变化于是轨迹在“往左一点”“往右一点”之间反复试探。先检查控制量加权矩阵 R 的量级把转向变化量从 0.1 压到 1 或 10横摆立刻减少。还有一个容易忽略的点相邻时间步的碰撞约束由不同的圆主导比如 t 时刻是前圆主导t1 时刻是左前圆主导。两个圆的梯度方向不同导致位置上出现锯齿。这种情况建议在状态代价里加一级位置平滑项或者把主导圆选择时的时间一致性做约束——不允许同一障碍物附近主导圆频繁切换。6.3 距障碍物很近了就是压不进安全距离乘子更新了几十轮最后停在 5 厘米误差怎么调都不动。我的经验是看惩罚系数是否过大而乘子更新过慢。AL 的收敛条件要求 \( \rho \) 足够大但如果乘子更新步长等于 \( \rho \times c \)在残差只剩 5 厘米时单次乘子增加也只有零点几而下一轮 iLQR 很可能又把轨迹调回来形成僵持。实战技巧在约束残差缩小时把外部迭代直接进入“乘子优先更新”模式即暂时减小外层求解的收敛阈值多推乘子或者干脆对残差小于阈值的约束做一次乘子放大。不要一个参数打天下。7. 验证闭环碰撞判定不能只看优化输出每次调完一圈参数我都会回放完整轨迹做三件事逐帧计算每个圆到每个障碍物的真实最小距离与优化器内部的距离函数对比。这一步可以发现平滑参数带来的偏置是否在工程可接受范围。检查车辆矩形包络与圆并集的差距——因为优化器只对圆约束负责矩形包络里可能存在没被圆覆盖的角点虽然撒点验证时没露但转角扫掠时可能会露。做闭环仿真而非开环回放。AL-iLQR 给的是离散控制序列真车或仿真器执行时会有跟踪误差。把优化解出来的轨迹作为参考在仿真器里加一点噪声重新跟踪看会不会擦碰。我遇到过三次“优化报告无碰撞台架测试刮碰”的情况前两次都是圆拟合漏了动态包络第三次是平滑距离函数在窄缝隙里高估了真实间隙。这三件事只要坚持做至少能过滤掉绝大多数几何层的问题。多圆拟合在 AL-iLQR 里本质上是对自车几何做的一次“可微分化妥协”。圆未必最精确但它能让优化器稳定地看到每条约束的梯度。这也是为什么我愿意花一整篇文章讲这个看似小节的环节——几何建模占了这类轨迹优化项目一大半的调试时间建模扎实了后面的碰撞约束、避让逻辑、调参工作才会顺。