做 AL-iLQR 轨迹优化的人都知道最磨人的往往不是优化器本身而是碰撞约束怎么“喂”给求解器。之前几篇实践指南里我陆续写过了状态估计、障碍建模、代价函数设计但到了真实车辆这个尺度自车轮廓这个问题就完全绕不过去了你不能把一个长 4.6 米、宽 1.9 米的矩形当质点用也不能拿矩形去做距离约束然后指望 iLQR 端到端求出一个又光滑又可行的轨迹。这次记录的是一个偏工程向的方案用多个固定圆贴合自车轮廓把碰撞约束转化成一组“圆到障碍物距离不得小于安全余量”的标量约束再全部塞进增广拉格朗日 iLQRAL-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前后轴各一前轴圆略大覆盖前端城区巡航3车尾、车身中部、车头车头圆半径较小突出前悬大角度转向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_{k1} X_k v_k * cos(θ_k) * dt Y_{k1} Y_k v_k * sin(θ_k) * dt θ_{k1} θ_k v_k / L_wb * tan(δ_k) * dt v_{k1} v_k a_k * dt δ_{k1} δ_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 典型调参配置和收敛判定下面这组参数是我在某个模拟项目里用了很久的配置可以作为初始值参考但不同场景一定需要微调参数推荐初值说明圆数量3泊车场景可减至 2safe_gap0.15 m控制误差预留平滑系数0.1平滑聚合时使用初始惩罚系数 ρ20太小收敛慢太大约束僵化乘子更新步长1.0随违反量调整外层迭代次数20不超过 50内层 iLQR 迭代10 到 15每次外层调用收敛判定不要只看总代价下降。我会同时看三件事约束最大违反量是否小于 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 乘子在大方向不对时会帮优化器找到局部修正但不要指望它能做全局拓扑变换。多圆碰撞约束在这套流程里承担的是“最后一公里”的精细化修正保证路径不刮擦、不留安全冗余而不是负责从零找出一条路来。我个人实际操作中最受益的一点是把碰撞约束的复杂度从矩形降到多圆以后很多以前躲不开的调试死角都变得清晰可见了。圆与圆的梯度是独立的乘子也是独立的哪个圆在哪个方向上有问题一眼就能定位。这种“可解释性”是这套方案最大的额外价值。等到后期想更进一步可以考虑把障碍物也拆成圆或者胶囊体让距离计算变成圆与圆的解析距离那样梯度更干净但碰撞精度会略微下降适合与本文方案做个对比验证看哪种更贴合你的实车表现。 
长河君 · 2026/10/22 14:53:44 · 阅读 1.2k 多圆拟合+AL-iLQR的自动驾驶车辆碰撞约束轨迹优化实践
做 AL-iLQR 轨迹优化的人都知道最磨人的往往不是优化器本身而是碰撞约束怎么“喂”给求解器。之前几篇实践指南里我陆续写过了状态估计、障碍建模、代价函数设计但到了真实车辆这个尺度自车轮廓这个问题就完全绕不过去了你不能把一个长 4.6 米、宽 1.9 米的矩形当质点用也不能拿矩形去做距离约束然后指望 iLQR 端到端求出一个又光滑又可行的轨迹。这次记录的是一个偏工程向的方案用多个固定圆贴合自车轮廓把碰撞约束转化成一组“圆到障碍物距离不得小于安全余量”的标量约束再全部塞进增广拉格朗日 iLQRAL-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前后轴各一前轴圆略大覆盖前端城区巡航3车尾、车身中部、车头车头圆半径较小突出前悬大角度转向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_{k1} X_k v_k * cos(θ_k) * dt Y_{k1} Y_k v_k * sin(θ_k) * dt θ_{k1} θ_k v_k / L_wb * tan(δ_k) * dt v_{k1} v_k a_k * dt δ_{k1} δ_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 典型调参配置和收敛判定下面这组参数是我在某个模拟项目里用了很久的配置可以作为初始值参考但不同场景一定需要微调参数推荐初值说明圆数量3泊车场景可减至 2safe_gap0.15 m控制误差预留平滑系数0.1平滑聚合时使用初始惩罚系数 ρ20太小收敛慢太大约束僵化乘子更新步长1.0随违反量调整外层迭代次数20不超过 50内层 iLQR 迭代10 到 15每次外层调用收敛判定不要只看总代价下降。我会同时看三件事约束最大违反量是否小于 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 乘子在大方向不对时会帮优化器找到局部修正但不要指望它能做全局拓扑变换。多圆碰撞约束在这套流程里承担的是“最后一公里”的精细化修正保证路径不刮擦、不留安全冗余而不是负责从零找出一条路来。我个人实际操作中最受益的一点是把碰撞约束的复杂度从矩形降到多圆以后很多以前躲不开的调试死角都变得清晰可见了。圆与圆的梯度是独立的乘子也是独立的哪个圆在哪个方向上有问题一眼就能定位。这种“可解释性”是这套方案最大的额外价值。等到后期想更进一步可以考虑把障碍物也拆成圆或者胶囊体让距离计算变成圆与圆的解析距离那样梯度更干净但碰撞精度会略微下降适合与本文方案做个对比验证看哪种更贴合你的实车表现。