企业官网建设流程全解析

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 圆布置：

对照这辆车的四个角点坐标：前保险杠左右角约在 (2.38, ±0.95)，后保险杠左右角约在 (-2.32, ±0.95)。前圆圆心 (1.85, 0) 半径为 0.55，最远覆盖到 x=2.40、y 到 ±0.55，显然盖不住前保险杠的左右角点。算法逻辑上，左右前圆就是来补这两个角的：左前圆圆心 (0.55, 0.75) 半径 0.45，它的最左最上边界能到 x=1.00、y=1.20……等等，这还不够靠近车头角点。

所以我要再强调，上面这组表只是“第一版草稿”，必须画图验证。验证方法很简单：在自车轮廓上均匀撒点，判断每个点是否落在至少一个圆内。如果某些点漏了，就调整对应圆的圆心或加大半径。这里给出我实际上会收敛到的 5 圆布置经验值：

这样前中圆的半径 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 左右。这个式子从来不会说出比真实最小距离更小的值，它只会轻微偏大，偏大方向对碰撞判定是安全的。实际测试里，\( \lambda=0.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 的局部二次目标只是标称代价与约束代价之和的二次近似。因为碰撞约束直接依赖位置状态，而位置状态一般只是从控制量积分来的，它的雅可比算起来也不难：

1. 正向仿真：从初始状态逐级积分，得到完整状态轨迹。
2. 约束计算：对每个时间步，用平滑后的圆约束函数计算 c_t 和偏导 \( \partial c_t / \partial x_t \)。
3. 反向传播：从末端倒着算 Q 函数、控制律与值函数。
4. 更新控制量：用新控制律做一次前馈加反馈更新。
5. 外层更新乘子。

关键细节在于乘子更新不能太激进。我用的是：对每一时间步，先计算“这个时间步有没有约束激活”，如果当前乘子加惩罚量之后约束仍然紧张，才更新；如果约束远未被激活，乘子退回 0。否则所有时间步的乘子都会缓慢增长，最后导致背景里到处都有微小的推挤力，轨迹质量反而下降。

5. 调参实战：权重、惩罚系数、平滑参数怎么配才稳

5.1 惩罚系数的启动值与倍增策略

我常用的初始配置是：

• 拉格朗日乘子初值 \( \lambda=0 \)；
• 惩罚系数 \( \rho \) 从 100 开始；
• 每两层外部迭代若约束残差没有降到设定阈值，\( \rho \) 乘以 4；
• 但如果约束残差连续两轮下降，就维持 \( \rho \) 不变。

为什么从 100 而不是更大的 10000 开始？因为初期 iLQR 还在调整全局轨迹，约束残差大是正常的，惩罚系数过大会立刻压制其他优化目标，导致轨迹偏离标称路径太远，后续纠正回来更难。而 100 这个量级，对于“露出边界 10 厘米”这种残差，产生的梯度足够让轨迹往安全区挪动，又不会把整个趋势带偏。

5.2 平滑参数 \( \lambda \) 与碰撞判定的关系

前面 4.1 节里用了 log-sum-exp 来平滑多边形的最近距离。这里的 \( \lambda \) 大小直接影响安全边界：

• \( \lambda=0.01 \) 时，平滑距离几乎等于真实距离，但梯度在顶点附近恢复陡峭；
• \( \lambda=0.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 时刻是前圆主导，t+1 时刻是左前圆主导。两个圆的梯度方向不同，导致位置上出现锯齿。这种情况建议在状态代价里加一级位置平滑项，或者把主导圆选择时的时间一致性做约束——不允许同一障碍物附近主导圆频繁切换。

6.3 距障碍物很近了就是压不进安全距离

乘子更新了几十轮，最后停在 5 厘米误差，怎么调都不动。我的经验是看惩罚系数是否过大而乘子更新过慢。AL 的收敛条件要求 \( \rho \) 足够大，但如果乘子更新步长等于 \( \rho \times c \)，在残差只剩 5 厘米时，单次乘子增加也只有零点几，而下一轮 iLQR 很可能又把轨迹调回来，形成僵持。

实战技巧：在约束残差缩小时，把外部迭代直接进入“乘子优先更新”模式，即暂时减小外层求解的收敛阈值，多推乘子；或者干脆对残差小于阈值的约束做一次乘子放大。不要一个参数打天下。

7. 验证闭环：碰撞判定不能只看优化输出

每次调完一圈参数，我都会回放完整轨迹，做三件事：

1. 逐帧计算每个圆到每个障碍物的真实最小距离，与优化器内部的距离函数对比。这一步可以发现平滑参数带来的偏置是否在工程可接受范围。
2. 检查车辆矩形包络与圆并集的差距——因为优化器只对圆约束负责，矩形包络里可能存在没被圆覆盖的角点，虽然撒点验证时没露，但转角扫掠时可能会露。
3. 做闭环仿真而非开环回放。AL-iLQR 给的是离散控制序列，真车或仿真器执行时会有跟踪误差。把优化解出来的轨迹作为参考，在仿真器里加一点噪声重新跟踪，看会不会擦碰。

我遇到过三次“优化报告无碰撞，台架测试刮碰”的情况，前两次都是圆拟合漏了动态包络，第三次是平滑距离函数在窄缝隙里高估了真实间隙。这三件事只要坚持做，至少能过滤掉绝大多数几何层的问题。

多圆拟合在 AL-iLQR 里，本质上是对自车几何做的一次“可微分化妥协”。圆未必最精确，但它能让优化器稳定地看到每条约束的梯度。这也是为什么我愿意花一整篇文章讲这个看似小节的环节——几何建模占了这类轨迹优化项目一大半的调试时间，建模扎实了，后面的碰撞约束、避让逻辑、调参工作才会顺。