AL-iLQR实践指南7自车多圆拟合与碰撞约束做轨迹优化的人应该都有过这种体验规划出来的路径明明避开了所有障碍物中心点但真车开过去右后视镜差点刮到护栏。问题就出在“自车”在算法里被当成一个质点或者一个简单矩形处理了。要把碰撞约束真正落到实车上第一步就是给自车一个足够贴近真实轮廓、同时又不让优化问题爆炸的几何表达。这一篇我重点讲多圆拟合怎么做以及如何在AL-iLQR框架里把这些圆变成真正起作用的约束。适合已经在跑轨迹优化、想让规划结果更接近实车可执行状态的朋友参考。我先把结论放前面在处理自车碰撞时多圆拟合是目前工程上兼顾精度、计算量和梯度平滑性的一个很实用的折中方案。配合增广拉格朗日迭代线性二次调节器AL-iLQRAugmented Lagrangian Iterative Linear Quadratic Regulator你可以把“车身不能碰到障碍物”这种非光滑、非凸的约束比较干净地嵌进优化求解过程里而且算出来的轨迹是可跟踪、可下发的。这篇不会讲太多玄乎的数学证明重点是讲清楚每一步怎么做、为什么这么做、以及我踩过的几个坑。1. 核心思路为什么是“圆”为什么是“多个”很多刚接触这个方向的人会先问自车明明是长方形为什么不用矩形包络矩形包络表达简单做纯几何检测时很好用但放进优化目标里就非常头疼。因为矩形的边界是由四条线段组成线段之间是角点距离场函数在角点处不光滑梯度方向会发生突变。AL-iLQR这类方法严重依赖梯度信息约束函数一旦不光滑求解器在迭代中很容易出现抖动、收敛变慢甚至发散。相比之下圆形的距离场表达式非常简单梯度处处可导对于优化求解器来说太友好了。但单个圆的包络太粗了车身稍微长一点一个圆要么把前后保险杠区域包进去一大块空余空间导致规划结果过于保守、在窄通道里根本找不到可行轨迹要么圆稍微缩小一点车头和车尾又露在圆外面碰撞检测就失真了。所以工程上自然而然的方案就是用多个圆沿车身纵轴排布用一组圆去近似矩形车身的外轮廓。这里需要明确一个概念多圆拟合不是为了“刚好包住车身”而是为了在“包络保守性”和“可用空间”之间取一个平衡点。圆越少约束越松求解越容易但包络越夸张圆越多几何表达越贴近真实车身但约束数量增加非线性程度变高求解难度也随之上升。实际项目中并不是圆越多越好而是要找到那个“够用”的数量。在我做过的项目中四米五左右的标准家用轿车用五个圆排布效果已经相当好。轴距两米七左右的车五圆的配置一般能覆盖从保险杠到后备箱的完整轮廓而且每个圆的半径不会差异太大约束条件相对均衡。车身特别长的厢式货车或者带挂车的组合才需要考虑七到九个圆甚至把挂车铰接点单独建模。1.1 多圆拟合背后的数学逻辑把自车简化为一组圆本质上是在做几何近似。每个圆需要一个圆心位置和一个半径。对于沿车身纵轴排列的圆来说圆心一般在车辆坐标系的x轴上也就是y坐标为零。圆心的x坐标取决于你要把圆放在车身的哪个位置半径则取决于该位置的车身宽度和你要预留的余量。假设车身长度为(L)车身宽度为(W)后轴中心为坐标原点车头方向为x轴正方向。如果你用(N)个圆去拟合最简单的一种方式是让圆心沿x轴等距分布然后根据车身两侧边界以及预留的膨胀距离来确定每个圆的半径。但这个等距方案在实际中往往不是最优的因为车身并不是一个完美的矩形——车头、车尾通常有弧度后视镜区域又明显更宽。如果你用统一的宽度去算每个圆的半径后视镜位置你就要多留出不少空间要么别的地方包络太紧。更实际的做法是先根据车身几何参数外壳点云或者外形图纸生成一个外轮廓多边形然后在每个待放置圆心的位置计算该横向截面上的半宽度再加上一个安全余量就得到该圆的半径。这个工作可以用简单的脚本半自动完成不必手动一个个去量。有个计算细节值得多说一句圆的半径不一定要取该位置车身半宽加余量因为圆是二维的它既要覆盖该截面位置的车身边界还要覆盖相邻截面位置的车身边界。如果你严格让每个圆都完全包住对应车身区段那么半径应该按照该圆覆盖区段内的最大半宽来计算。实际操作中我一般会先排好圆心再给所有圆的半径同时乘一个1.05到1.15的系数把这个修正项吸收掉然后做一次全轮廓校验看车身边界是否都落在圆内。校验不通过就调系数或者微调圆心位置。1.2 圆的数量和位置怎么确定圆的参数没有唯一标准答案我提供一个可复用的调参流程自己在项目中按这个顺序走基本一次就能得到比较稳定的结果先确定总长和轴距把车头和车尾的悬垂部分也标出来。在纵向上按车头悬垂、前轮、后轮、车尾悬垂这几个关键位置布点一般这些点就是圆心的候选位置。在候选点处量车身横向半宽加上你的安全余量比如0.1米到0.15米得到初始半径。用所有圆做一个整体包络检验把车身轮廓离散成密集点云逐点判断是否在某个圆内。如果有车身点在所有圆之外要么增加圆的半径要么在附近增加一个圆要么调整圆心位置。检查圆的包络外轮廓是否过度超出车身边界尤其是前后保险杠附近。如果某些区域包络超出太多说明该区域覆盖半径过大需要将该处圆向相邻区域移动或者减少该圆的半径、增加一个额外圆来分担覆盖。最后记得把后视镜宽度考虑进去。后视镜是车身最宽的地方也是最容易发生刮擦的区域。很多项目在初版拟合时会漏掉后视镜结果仿真里碰撞约束都过了实车一跑后视镜先撞了。后视镜区域的处理方法有两种一是单独把这个区域的圆半径加大二是直接在约束计算里对后视镜区域做额外的碰撞检测。第一种方式更简单但会让包络变大窄通道通过性变差第二种方式更精确但约束逻辑更复杂。我个人建议在规划阶段先用第一种保证求解稳定后续有条件再做精细化处理。2. 碰撞约束在AL-iLQR里的定位有了自车的多圆表达还需要回答一个问题这些圆如何参与轨迹优化AL-iLQR本身是一个求解最优控制问题的框架它的核心迭代机制分成内外两层内层是ILQR负责在没有复杂约束或只带简单惩罚的情况下快速求解一个局部最优轨迹外层则是增广拉格朗日迭代把原问题中的复杂约束通过拉格朗日乘子和惩罚项增广到目标函数里再交替更新乘子和惩罚系数让求解结果逐步逼近满足约束的可行解。这里要理解一个关键点AL-iLQR并不要求你把所有约束表达成光滑的等式或者不等式。它非常适合处理那种“你不确定能让约束严格满足到多高精度但你又希望约束尽量被满足”的场景。碰撞约束就是典型代表。你可以把碰撞约束写成障碍物圆和自车圆之间最小距离大于等于某个安全距离但这个约束在大部分时间里是不活跃的只有车辆靠近障碍物时才起作用。如果你把它作为硬约束直接塞进去求解器的有效集切换会导致迭代过程很不稳定用增广拉格朗日做软优化靠近障碍物时约束自然被“顶起来”远离时也不会拖累求解。在具体处理时我会把碰撞约束分两层来看第一层是几何计算层。给定当前轨迹上的车辆位姿x、y、航向角用自车多圆的位姿变换关系算出每个圆心的全局坐标然后对每个圆心找到所有障碍物中距离最近的那个障碍物圆记录距离值。第二层是约束构建层。把“距离小于安全距离”的部分变成惩罚量进入增广拉格朗日目标函数。距离越近惩罚越大梯度方向指向远离障碍物的方向。2.1 为什么不用纯惩罚函数如果你用纯罚函数处理碰撞约束也就是只在目标函数里加一个“穿透深度平方”的惩罚项那么问题在于惩罚系数太小约束会被突破惩罚系数太大目标函数会变得非常畸形原本要跟踪的参考路径和保持舒适性的成本项都被淹没轨迹会变得激进。而且纯罚函数方法没有显式地记录“这个约束有多难满足”每次求解都要重新调参。增广拉格朗日的做法比罚函数聪明的地方在于它引入了一个乘子项乘子会随着迭代不断累积。第一次迭代约束没满足乘子就变大下一次迭代惩罚的力量就更强。这样一来同一个惩罚系数在多次迭代中可以被“逐步加压”最终既能满足约束又不至于把目标函数搞得面目全非。这也是为什么在AL-iLQR中碰撞约束倾向于写成带乘子的增广形式而不是简单的大权重罚函数。实际操作中我习惯把碰撞约束的不满足量定义为[ g_i d_{\text{safe}} - d_i ]其中(d_i)是自车某个圆到最近障碍物圆的距离(d_{\text{safe}})是安全距离。约束目标是(g_i \le 0)。如果(g_i 0)表示约束被违反。增广拉格朗日目标中对应项的写法是[ \Phi_i \mu_i g_i \frac{\rho}{2} g_i^2 ]其中(\mu_i)是拉格朗日乘子(\rho)是惩罚系数。外层迭代时(\mu_i)按(\mu_i \leftarrow \mu_i \rho g_i)更新。这个形式的好处是“惩罚”是带方向的约束违反得越狠、乘子增长得越快而一旦约束被满足乘子就停止增长不会像纯罚函数那样有过冲。2.2 多圆之间如何统一约束自车有多个圆每个圆都是独立的碰撞体理论上每个圆都要和所有障碍物做距离检查。这样一来约束数量就不可控了如果自车有5个圆场景里有20个障碍物那就是100个约束对而且每个约束的梯度都要参与回传求解负担不小。我的做法是分两步压缩第一步做邻域筛选。在优化迭代的当前轨迹附近先做一次粗略的包围盒检查把离自车所有圆都一定距离以上的障碍物直接剔除不参与约束计算。第二步做“该轨迹点上每个圆只关联最近障碍物”的处理。也就是在同一个控制时刻上对于每一个自车圆只找距离最近的那一个障碍物圆作为约束对象。如果一个障碍物已经约束了前保险杠对应的圆那它就自动不会再去约束车厢中部的圆除非确实该障碍物同时威胁到两个圆。这种就近匹配策略会带来一个梯度切换问题在迭代过程中某个自车圆的最近障碍物可能会从A切换到B导致约束函数在一瞬间不是光滑的。解决办法有两个方向一是在切换点附近增加一个平滑过渡的权重函数让两个障碍物的影响按距离加权融合二是接受这种非光滑性因为增广拉格朗日外层迭代本身对单次ILQR的精确解依赖度并不是那么高有一点点切换抖动通常可以被后续迭代修正。我的项目里用的是方案二实测下来收敛性没有受到明显影响。3. 实操把多圆约束写进求解循环接下来这部分是真正可以拿去直接用代码实现的思路。我给出一个比较通用的实现结构不绑定具体框架。3.1 自车圆的位姿传递假设轨迹节点上车辆位姿为((x_t, y_t, \theta_t))。自车圆的圆心在车辆坐标系下是((px_i, 0))半径是(r_i)。那么全局坐标就是[ cx_{t,i} x_t px_i \cos\theta_t ] [ cy_{t,i} y_t px_i \sin\theta_t ]这里我默认自车圆都在车辆纵轴上也就是车身坐标系的y坐标为0。如果你的拟合结果里有非零y坐标的圆把这个公式扩展成二维旋转即可并不复杂。注意车辆坐标系方向定义要统一别在符号上引入混乱。3.2 障碍物圆的距离计算障碍物在场景里通常被表达成带半径的圆。求两个圆的最短距离就是圆心距离减去两个半径之和然后取与0的最大值。当距离为正时没有物理碰撞当距离为负时表示发生了重叠。那么约束中需要的(d_i)可以写成[ d_i \sqrt{(cx_{t,i} - ox_j)^2 (cy_{t,i} - oy_j)^2} - r_i - ro_j ]其中((ox_j, oy_j))为最近障碍物圆的圆心(ro_j)为障碍物圆半径。这一段计算在整个约束函数里是核心因为后面所有梯度都要从它推导出来。求导时注意根号项对(cx)和(cy)的偏导形式很多初写代码的人在这里会漏掉链式法则。3.3 梯度推导与实现既然要写进ILQR那每一时刻的控制量和状态量都要能算梯度。多圆碰撞约束涉及的状态量主要是(x_t)、(y_t)、(\theta_t)。为了简化我把某个圆对应的约束违反量记为(h_t)它关于状态量的梯度需要用到链式法则。具体计算时先确定当前障碍物圆圆心((ox, oy))然后定义[ dx cx_{t,i} - ox,\quad dy cy_{t,i} - oy ] [ dist \sqrt{dx^2 dy^2} ]其中(cx_{t,i}、cy_{t,i})又是(x_t, y_t, \theta_t)的函数。于是[ \frac{\partial h_t}{\partial x_t} \frac{\partial h_t}{\partial dx} \frac{\partial dx}{\partial x_t} \frac{\partial h_t}{\partial dy} \frac{\partial dy}{\partial x_t} ]注意(\frac{\partial dx}{\partial x_t} 1)而(\frac{\partial dy}{\partial y_t} 1)。对(\theta_t)的偏导更啰嗦因为(cx)和(cy)都受(\theta_t)影响需要额外写一个小的雅可比矩阵计算函数。我这里推荐一个习惯不要手推完所有公式再写代码而是先用数值差分去校验解析梯度的符号和大小。别觉得这是小题大做碰撞约束的梯度往往是负号错一点整个轨迹就反着跑了。在我自己的实现里解析梯度写完后会做一个随机位姿测试用数值差分对比最大误差。误差阈值我一般设1e-4超过这个值就说明推导或实现有误立刻排查不要等到整个优化器跑起来再对着一堆奇怪的轨迹发呆。3.4 增广拉格朗日外层更新的实现细节增广拉格朗日的核心更新公式不复杂但有几个细节影响收敛速度。我在项目里用的是这个流程初始化所有乘子(\mu_i 0)惩罚系数(\rho \rho_0)。进入内层ILQR求解得到当前乘子下的最优轨迹。检查所有约束违反量记录最大违反量。如果最大违反量小于容差结束否则更新乘子和惩罚系数重新开始内层求解。惩罚系数的更新策略要稍微注意。如果每次迭代都让(\rho)暴增比如乘以10甚至50内层ILQR的目标函数形态变化很剧烈前一轮求出的轨迹基本上全作废收敛反而慢。我一般用(\rho \leftarrow \rho \times 2)最多不超过4让惩罚逐步加压。乘子更新则按照标准公式(\mu_i \mu_i \rho g_i)。如果约束持续改善但尚未满足乘子会越积越大这就是增广拉格朗日能实现精确约束的关键。3.5 多圆约束的计算压力优化轨迹优化往往要跑几十到上百轮迭代每一轮都要重复计算障碍物距离。如果你在每轮都对所有障碍物做全量扫描场景稍微复杂一点性能就崩了。我建议在ILQR的外层、也就是位姿信息相对稳定的时候先构建一个空间索引结构。这里不需要用太复杂的库一个简单的均匀网格或者四叉树就够用。然后在每一轮ILQR中只对当前轨迹点附近网格内的障碍物做距离计算。这样可以从多个轨迹点共享缓存大幅减少重复计算。不过要记得在轨迹发生较大变化时更新索引否则可能出现漏检。4. 参数调优与碰撞约束的工程细节4.1 安全距离怎么设安全距离不只是“别碰上去”的最小距离它还包含了控制误差、感知误差和车辆执行延迟带来的不确定性。我常用的做法是把安全距离分成两部分第一部分是几何解耦距离也就是自车圆半径中已经包含的膨胀量这部分一般取0.05到0.15米。第二部分是动态余量与当前车速、转向速率、感知延迟相关。车速越高这个余量越大。举个例子如果我规划的车速是10米每秒感知延迟0.2秒光延迟引起的附加距离就是2米。这个数量级对近距离泊车等场景显然不合理。所以在实际项目中我会把安全距离定义为一个与速度相关的函数低速场景给很小值高速场景给较大值。一个比较实用的分段函数[ d_{\text{safe}}(v) d_{\text{base}} k_v \cdot |v| ](d_{\text{base}})为低速安全距离(k_v)为速度系数。假如(d_{\text{base}}0.3)米(k_v0.05)车速5米每秒时安全距离就是0.55米车速15米每秒时安全距离是1.05米。这个公式简单也方便调参。4.2 障碍物圆的膨胀处理感知系统给出的障碍物轮廓通常是不规则多边形直接做点集结合计算会很麻烦。工程上常见做法是先对每个障碍物做外包络圆再用一个或几个圆去近似。这个过程本质也是拟合只是方向不同。如果障碍物本身比较狭长比如路边停了一排车单个大圆包络会占用大量可行空间。我一般会让感知输出一个“主圆加若干子圆”的表达方式主圆覆盖障碍物中心区域子圆覆盖两端突出部分。这样在轨迹优化里自车圆与每组障碍物圆的距离约束仍是确定性的不会引入额外随机性。这里要注意障碍物圆的半径同样存在膨胀量。障碍物膨胀过大会导致自车在窄路中无法通行膨胀过小又有碰撞风险。一般来说静态障碍物膨胀量可以设得比较小比如0.1到0.2米动态障碍物由于预测不确定性高膨胀量要更大有时甚至直接按整个预测走廊的包络来处理。4.3 约束可信度的量化AL-iLQR一个额外的好处是乘子的大小其实可以当做一个“约束紧张程度”的指示器。如果某个乘子非常大说明对应的碰撞约束很难满足那个区域是不是真的过不去这时候我会检查该约束所对应的障碍物圆和自车圆的几何关系看是否存在过于保守的包络。举一个我实际遇到过的例子某次停车场场景中约束一直无法满足乘子涨到很大轨迹被推得贴到地图边界。后来排查发现问题出在一个障碍物圆的膨胀量设得过大那个位置本来可通行宽度是够的但因为障碍物膨胀多占了0.3米导致自车圆和障碍物圆的距离一直小于安全距离。把膨胀量降下来后约束很快收敛。这提醒我一旦某个约束顽固不化先别怀疑求解器回头检查几何表达是否合理。5. 常见问题与避坑经验从算法调试到实车验证这个模块我遇到过不少问题这里挑几个有代表性的写出来供大家参考。5.1 轨迹出现来回摆动如果算法跑出来的轨迹在障碍物附近来回摆动而不是平滑绕开通常有几个原因。第一个原因是约束梯度在障碍物两侧不对称。因为碰撞约束用的是“最近障碍物”匹配轨迹在障碍物左侧时左边圆被约束轨迹在右侧时右边圆被约束。但如果你车头、车尾的圆排布不对称或者圆的半径没有让车身形成对称包络梯度就会失衡轨迹会被来回推。第二个原因是惩罚系数太大。增广拉格朗日的惩罚项一旦过于激进每一轮ILQR的轨迹都会在约束边界附近振荡。解决方法是适当降低(\rho)的递增系数同时增加乘子的作用让约束靠乘子逐步累积去满足而不是靠单次大权重硬压。第三个原因更隐蔽是ILQR内层收敛不彻底外层乘子更新的时刻轨迹并不是真正的局部最优解。这时候乘子可能是在一个次优点上更新方向不准轨迹自然乱摆。解决方法是提高内层ILQR的迭代收敛条件至少让梯度范数下降到一个阈值后再进入外层更新。5.2 多圆拟合的包络漏检自车多圆拟合后一定要做可视化检查别只跑一个“所有点都在圆内”的数学判断。我吃过亏的点在于车身外轮廓的离散点密度不够车顶边缘和保险杠下方有一个弧形过渡区域点云稀疏时判断不出漏洞等到高密度点云校验时才发现。所以校验点的密度不能按经验拍脑袋直接按每10厘米一个点采样就差不多了。还有一个容易被忽略的地方车辆在转向时车身轮廓并不是刚体平移前轮转向会让前悬部分发生小幅摆动这个摆动在多圆拟合时并没有体现。如果你做的是紧急避障类工况前悬的位置变化会比较大可能要额外给前悬对应的圆加一点余量。5.3 约束无法完全满足到最后总能收敛吗在AL-iLQR中如果惩罚系数和乘子迭代到位理论上是可以让约束以很高精度满足的但实际工程中不必追求零违反。我一般设置一个容差比如最大违反量小于0.05米就认为约束可接受。原因是内层ILQR求解的是局部最优过分追求约束精度容易让轨迹质量变差出现猛打方向等不舒适行为。如果多次外层迭代后违反量停留在某个值附近降不下去而增大惩罚系数也没有明显效果这时候八成是约束本身没有可行解也就是说在当前位置、当前自车表达下确实没有任何轨迹能满足所有碰撞约束。这不是求解器的问题建议直接调整参考路径或者放宽安全距离换个工况重新规划。5.4 动态障碍物的碰撞约束注意事项我们这套多圆约束在静态场景下效果很好但动态障碍物需要另外处理。如果直接在每个时间节点上把动态障碍物当成固定圆规划出来的轨迹会非常僵硬因为它没有利用“障碍物也在移动”这个信息。我的做法是给动态障碍物多圆加入时间维度在每一时刻使用对应时刻的预测位置并在安全距离里额外叠加一个预测置信度相关的余量。如果预测置信度低比如混乱路口就把膨胀半径调大如果预测置信度高比如高速直线行驶的车辆膨胀半径可以小一些。AL-iLQR的时间维度天然支持这种逐时刻变化的参数实现起来并不复杂。5.5 数值问题当自车圆和障碍物圆严重重叠在某些起步或者终止状态下自车圆可能和障碍物圆已经发生了严重重叠这时候距离(d_i)为负值且绝对值很大约束违反量(g_i)很大梯度方向会变得非常剧烈。ILQR的线性化在这种区域容易失真导致一步就冲出去很远的量。我的处理方法是给约束函数套一个上限或者软化因子比如让约束函数在违反程度大于某个值后增长速度放缓。具体来说可以不用纯粹的线性违反而是用一个软加函数[ g_{\text{soft}} s \cdot \log(1 e^{g/s}) ]其中(s)是软化尺度。当(g)很大时这个函数增长速度接近线性但不至于爆炸当(g)为负时它趋近于0不会产生不必要的惩罚。这个技巧在拥挤场景或者启停工况中很管用能明显提升ILQR的稳定性。6. 实操总结与一套可复用的配置参考为了让前面内容落地更轻松我把一套经过测试的配置直接列在这里。这套配置适用于车身长度4.5米左右、宽度1.85米左右的普通家用车泊车或城区低速场景居多。参数项推荐配置说明自车圆数量5覆盖车头悬垂、前轮、中部、后轮、车尾自车圆半径范围0.45米到0.75米头部和尾部半径偏小中部偏大圆心x分布-1.7米到1.9米以后轴中心为0均匀排布并按轮廓微调安全距离基准值0.15米到0.3米低速停车场取小值结构化道路取大值速度系数0.03到0.08对应动态余量按执行器延迟调整增广拉格朗日初始惩罚系数0.1过大容易振荡过小收敛慢惩罚系数递增倍率2.0每次外层迭代乘以2最多乘到10约束容差0.03米到0.05米适用于常规城市工况障碍物圆膨胀量0.1米到0.2米静态障碍物取下限动态取上限这组参数不是万能的但它能作为一个可靠的起点。你拿它跑完全流程再根据你的车辆几何和工况微调会节省很多盲调的时间。在调试时我还建议做一件事把自车多圆和障碍物圆的可视化写到日志里。每一轮外层迭代结束后保存一张俯视图图上画出当前轨迹、自车圆、障碍物圆和碰撞约束的几何关系。这样你能非常直观地看到约束是怎么被推开的哪些区域一直紧贴边界哪些约束形同虚设。这种可视化调试对于理解AL-iLQR的收敛过程、排查约束问题非常有帮助。最后说一个我非常推荐的扩展方向把多圆拟合的结果应用到实车的局部规划模块里同时和底盘的状态反馈做闭环。我们在仿真里跑通只是第一步真正考验多圆拟合和碰撞约束的是坡道、窄道、有绿化带和路沿石的真实场景。在这些场景里感知输出的障碍物圆往往带有噪声约束的鲁棒性比求解速度更值得花时间去打磨。我个人在实际测试中的体会是优化出来的轨迹其实已经能避开大部分碰撞风险真正的风险反而在于感知噪声在梯度计算中被放大导致实车轨迹频繁修正。后来我在自车圆半径中加入了针对感知噪声的膨胀项同时降低了对极近距离障碍物的梯度权重实车表现才明显稳定下来。这个技巧听起来简单但救了我很多次调试。如果你正在做类似的项目不妨先在自己的仿真环境里模拟一下带噪声的障碍物圆看看AL-iLQR跑出来的轨迹会不会随着噪声出现不应有的抖动。能抗住这一层的约束才是可以在实车上放心用的约束。
