做 AL-iLQR 軌跡優(yōu)化的人都知道最磨人的往往不是優(yōu)化器本身而是碰撞約束怎么“喂”給求解器。之前幾篇實踐指南里我陸續(xù)寫過了狀態(tài)估計、障礙建模、代價函數(shù)設計但到了真實車輛這個尺度自車輪廓這個問題就完全繞不過去了你不能把一個長 4.6 米、寬 1.9 米的矩形當質點用也不能拿矩形去做距離約束然后指望 iLQR 端到端求出一個又光滑又可行的軌跡。這次記錄的是一個偏工程向的方案用多個固定圓貼合自車輪廓把碰撞約束轉化成一組“圓到障礙物距離不得小于安全余量”的標量約束再全部塞進增廣拉格朗日 iLQRAL-iLQR框架里解。這個做法不是新潮玩意但確實是我在實際項目里反復對比下來最穩(wěn)定、最好調參、也最容易定位問題的一套組合。如果你已經在做 iLQR 相關的軌跡規(guī)劃或者正準備從質點約束往整車約束升級這篇文章應該能讓你少踩幾個坑。1. 自車多圓擬合碰撞幾何的拆解與選圓1.1 為什么不能簡單把車當成質點或矩形硬算先講清楚我為什么棄用“質點 障礙膨脹”的方案。質點的好處是約束容易寫一個點到所有障礙物的距離不小于某個閾值就完事。但真實車輛的橫向寬度和前后懸擺在那兒尤其在小半徑轉彎場景里車尾會往外甩、車頭會往內切用質點模型去近似實際執(zhí)行時經常出現(xiàn)“優(yōu)化認為很安全實車卻已經貼到障礙物”的情況。為了彌補這一點去暴力膨脹障礙物又會讓狹窄通道直接變不可行得不償失。那直接用矩形做碰撞約束呢理論上當然最精確但工程上非常痛苦。點到矩形的距離函數(shù)本身是分片函數(shù)最近點可能落在邊上的任意位置也可能落在頂點上求導和線性化都需要做大量分類討論。更關鍵的是iLQR 在每一輪迭代里都會對約束做一階或二階近似約束函數(shù)本身不夠光滑優(yōu)化器就容易在矩形邊緣附近震蕩甚至收斂到數(shù)值上不可用的軌跡。多圓擬合的思路介于兩者之間用幾個半徑固定的圓去覆蓋整車的矩形包絡把“自車碰到障礙物”等價成“這些圓中任何一個的圓心到障礙物邊界的最短距離小于對應半徑”。于是碰撞約束從“矩形到多邊形的距離”變成了“若干個圓心到障礙邊界距離”的并聯(lián)約束。每個距離都可以用點到線段求交來算求導結構簡單、梯度連續(xù)性好而且天然支持增量更新非常適合反復迭代的優(yōu)化器。1.2 車輛輪廓的圓擬合實際構造方法在做擬合之前先把車的幾何說清楚。我通常用一個簡化包圍矩形描述車輛車頭到前軸有一段前懸前軸到后軸是軸距后軸到車尾是后懸再加上左右兩側的寬度。如果項目里沒有精確數(shù)據(jù)用一個典型尺寸就可以總長約 4.7 米總寬約 1.9 米軸距約 2.8 米前懸 0.9 米后懸 1.0 米。在這個矩形里“種”圓最常見的是對稱布置。圓的數(shù)量從 2 到 5 不等數(shù)量越多越貼合矩形但對應的約束數(shù)量也越多優(yōu)化求解負擔相應變大。我的默認配置是 3 個圓后軸中心附近放一個半徑較小的圓覆蓋車尾區(qū)域前軸中心附近放一個半徑稍大的圓覆蓋車身中部車頭前方再放一個圓照顧車輛轉向時的頭部擺幅。如果車輛軸距特別長或者經常做大幅度轉彎我會在中間加第 4 個、第 5 個圓保證轉彎掃掠包絡不被漏檢。每個圓的圓心坐標和半徑怎么定我一般不是純手調而是把問題寫成一個小的最小最大優(yōu)化設定圓心坐標沿車身縱向偏移為變量半徑統(tǒng)一或分別獨立目標是最小化所有圓覆蓋矩形后露出的最大“缺口”同時要求圓不能超出矩形邊緣太多否則會把安全區(qū)域憑空擴大。實際工程里其實不需要追求嚴格最優(yōu)只要保證兩件事矩形四個角盡量被圓蓋住以及相鄰圓之間沒有明顯缺口。這個用畫圖軟件看一圈就能調出來比上優(yōu)化器更快。1.3 圓數(shù)量與包絡精度的取舍圓不是越多越好。我試過 7 個圓的方案包絡確實嚴絲合縫但優(yōu)化時間幾乎翻倍而且多余的約束在寬度方向上有很強的冗余導致增廣拉格朗日乘子之間相互牽扯最后收斂速度反而變慢。個人經驗是普通城區(qū)低速泊車場景用 2 到 3 個圓就夠結構化道路變道場景加 1 個車頭圓需要原地旋轉或頻繁大轉向的場景才建議 5 個圓。這里有一個直觀的類比車輛轉彎時車身掃過的面積并不是一個簡單矩形更像一個向內收縮、向外擴張的“掃掠帶”。多圓擬合并不能保證每一瞬間都完全貼合掃掠包絡但可以在關鍵位置兜住車輛外輪廓。你只需要保證“圓覆蓋區(qū)域始終包含真實車輛包絡”碰撞約束的安全性就成立了。我常用的三類配置如下表方便直接抄場景圓數(shù)量圓心分布半徑經驗值低速泊車2前后軸各一前軸圓略大覆蓋前端城區(qū)巡航3車尾、車身中部、車頭車頭圓半徑較小突出前懸大角度轉向5前后軸各一加前中后補位中間圓半徑最小補缺口需要強調一點擬合圓的半徑應該比車輛半寬略大多出來的部分作為基礎安全余量。這個余量和后面講的碰撞安全距離是兩個概念前者是幾何近似誤差后者是規(guī)劃器對控制偏差和感知誤差的預留兩者不能混為一談。2. 碰撞約束的 AL-iLQR 建模2.1 iLQR 與增廣拉格朗日的基本氣質先說 iLQR 的核心邏輯。iLQR 會圍繞當前軌跡將系統(tǒng)動力學線性化、將代價函數(shù)二次化然后通過逆向傳播得到一條局部最優(yōu)的反饋控制序列再正向仿真更新狀態(tài)軌跡。這個流程反復迭代直到軌跡收斂。它的優(yōu)勢是計算結構基本上是矩陣乘法和回代實時性好短板是處理復雜不等式約束比較繞。躲障礙物本質就是一堆不等式約束位置必須保持在可行空間里。直接把這些約束硬加進 LQR 里會讓原本清爽的線性二次結構被破壞掉。常見的替代思路有三種純懲罰法、約束投影法、增廣拉格朗日法。純懲罰法簡單但罰系數(shù)大了容易奇葩、小了約束無法嚴格滿足投影法在約束邊界復雜時容易來回彈跳增廣拉格朗日法是在原來的目標函數(shù)上加一項“帶乘子的二次懲罰”然后用外層循環(huán)更新乘子。AL-iLQR 把增廣拉格朗日思想裝進 iLQR 迭代里等于把不等式約束拆成兩部分處理內層 iLQR 只解一個無約束或只帶簡單約束的最優(yōu)控制問題外層負責更新乘子和罰系數(shù)。這樣做的好處是既保留了 iLQR 的高效數(shù)值結構又能讓最終解在滿足約束的方向上不斷被“拽”回去。實際項目里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。這樣代碼里只需要維護一個“等效半徑”約束形式更統(tǒng)一。為什么用“點到線段”而不是“點到多邊形”因為線段距離的梯度特別干凈最近點要么落在線段內部此時梯度方向就是圓心到垂足的方向要么落在線段的兩個端點之一此時梯度方向就是圓心到端點的方向。這個邏輯寫滿也只是幾個 if 分類不會出現(xiàn)高階非光滑項傳給 iLQR 的雅可比矩陣非常友好。2.3 多圓并聯(lián)約束與平滑聚合一個自車可能有 3 個圓一個圓周圍可能有 5 個障礙物。那么理論上的碰撞約束應該是“所有圓到所有障礙物的距離中的最小值大于安全值”。但如果直接把取最小值這個操作放進約束公式里約束函數(shù)在多個圓之間會發(fā)生切換產生尖點也就是不可導點。為了避免這種切換我通常用兩種方式處理第一種是“分離約束”也就是不取最小值而是把每個圓對每個障礙物的約束都單獨寫成一行不等式約束。這樣約束數(shù)量會多一些但每個約束都是光滑的乘子更新也方便。缺點是當障礙物很多時約束矩陣會膨脹。第二種是“平滑聚合”用類似 LogSumExp 的平滑最小函數(shù)去近似所有距離中的最小值。這樣做約束數(shù)量少但平滑參數(shù)不能調得太大否則會出現(xiàn)“看起來約束滿足實際已經穿透”的假象。我的建議是地圖中障礙物數(shù)量少時用第一種把每個圓每塊障礙的約束都顯式寫出來方便排障稠密交通流里用第二種減少約束數(shù)量換計算速度。如果用的是平滑聚合平滑系數(shù)的取值要在“梯度連續(xù)”和“約束保守性”之間找平衡。我一般把平滑系數(shù)設為 0.05 到 0.2 之間具體取決于車輛尺寸。3. 實操實現(xiàn)從幾何到乘子的完整閉環(huán)3.1 狀態(tài)、控制與車輛動力學定義我的 AL-iLQR 里狀態(tài)量取 x [X, Y, θ, v, δ]?控制量取 u [a, ω]?。X、Y 是車體參考點通常取后軸中心在世界坐標系下的位置θ 是航向角v 是縱向速度δ 是前輪轉角。a 是縱向加速度ω 是前輪轉角速率。離散時間模型我常用一個符合低速泊車和城區(qū)巡航場景的簡化自行車模型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 是軸距。注意這里車的參考點在后軸中心所以圓心的位置要從狀態(tài)量里推導出來。比如第 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(θ)圓心位置是狀態(tài)量的函數(shù)所以距離約束對狀態(tài)的梯度非常自然直接走鏈式法則就能得到。這也是多圓擬合方便落地的核心原因幾何上只是加一個坐標變換代碼上完全不需要改 iLQR 的主流程。3.2 距離約束的雅可比求解細節(jié)要讓 iLQR 正確利用碰撞約束需要拿到每個約束對狀態(tài)量的一階梯度。對某個圓來說先求圓心到障礙線段的最短距離 d并記錄最近點 q 在線段上的參數(shù)位置。如果最近點在線段內部那么從圓心到垂足的方向就是約束梯度方向如果最近點是某個端點約束梯度方向就是從圓心到該端點的方向。這個方向向量記為 n_hat然后對圓心位置 p 求導是 n_hat再對狀態(tài) x 求導就是 ?p/?x 與 n_hat 的復合。?d/?x n_hat? * ?p/?x?p/?x 里面無非是 cos、sin 和偏移量這個推導我建議用手推一遍不要依賴自動微分。原因有一個 iLQR 每次迭代都要把約束對狀態(tài)求導如果能手寫出解析梯度代碼執(zhí)行效率會高一個量級。自動微分雖然方便但在這種強實時任務里沒必要把性能讓出去。還有一個細節(jié)如果圓心到線段的最短距離出現(xiàn)在線段延長線上距離公式仍然成立但梯度方向會有微小的誤差。工程上我會做一個小的截斷處理把參數(shù) s 限制在 [0, 1] 范圍內再從端點取梯度。這相當于在距離函數(shù)里引入了一個很小的近似的噪聲但對優(yōu)化收斂幾乎沒有影響。3.3 AL 內外循環(huán)結構和乘子更新AL-iLQR 的實現(xiàn)我習慣分兩層循環(huán)。外層循環(huán)更新增廣拉格朗日的乘子 λ 和懲罰系數(shù) ρ內層循環(huán)用 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) 只對違反約束的部分做懲罰滿足約束的部分不疊加額外代價這樣比較自然。外層循環(huán)每輪結束以后用當前違反量更新乘子λ_i max(0, λ_i ρ * max(0, g_i))同時如果違反量下降緩慢把 ρ 乘一個 1.5 到 2.0 的倍率如果違反量已經小于閾值就維持 ρ 不變防止過大的 ρ 導致軌跡剛硬化。我遇到過有人把 ρ 一開始就設成 1000然后發(fā)現(xiàn)軌跡被懲罰項壓得筆直寧可繞大遠路也不敢貼障礙物路線質量非常差。更好的做法是把 ρ 從 10 或者 50 起步讓軌跡先自由優(yōu)化出一個大方向再逐步用乘子和罰系數(shù)把軌跡拉進可行域。這個順序非常重要順序反了很容易讓整個軌跡陷入局部極小。3.4 典型調參配置和收斂判定下面這組參數(shù)是我在某個模擬項目里用了很久的配置可以作為初始值參考但不同場景一定需要微調參數(shù)推薦初值說明圓數(shù)量3泊車場景可減至 2safe_gap0.15 m控制誤差預留平滑系數(shù)0.1平滑聚合時使用初始懲罰系數(shù) ρ20太小收斂慢太大約束僵化乘子更新步長1.0隨違反量調整外層迭代次數(shù)20不超過 50內層 iLQR 迭代10 到 15每次外層調用收斂判定不要只看總代價下降。我會同時看三件事約束最大違反量是否小于 0.02 米、軌跡終點的位置誤差是否小于給定閾值、相鄰兩次迭代的控制量變化是否足夠小。三者同時滿足才算收斂。有一點要提醒AL 方法允許中間軌跡違反約束這是正常行為。你看到第 5 輪迭代時軌跡還穿模別急著加罰系數(shù)先看乘子是否在增長。乘子增長說明約束正在被納入優(yōu)化目標這是好現(xiàn)象如果乘子不動但軌跡在抖那多半是梯度計算有 bug。4. 避坑與排查記錄4.1 圓包絡本身的安全余量嚴重不足我最早做多圓擬合時為了讓包絡更貼合矩形把圓半徑壓得很緊結果仿真中經常出現(xiàn)“圓沒碰到障礙物但車輛角點已經刮到”的情況。原因是車輛真實包絡和圓包絡之間總會有擬合誤差哪怕只有幾厘米在車輛做大轉角時也會被放大。后來我改成每個圓在擬合半徑基礎上再加一個 inflation 量。具體做法是把圓的等效半徑設定為擬合半徑 0.08 米并且把這個值作為約束的一部分直接帶入計算。這樣做的代價是通行寬度會被吃掉一點點但換來的是碰撞零容忍場景下的穩(wěn)定安全。要記住多圓擬合是一種保守近似寧可多占一點安全距離也不要賭擬合誤差的方向。4.2 距離函數(shù)光滑化過度導致約束穿透用平滑聚合時有一個非常典型的坑平滑系數(shù)越大梯度越平緩優(yōu)化器越容易收斂到局部解但平滑后的“最小距離”會比真實最小距離偏大于是車輛實際上已經離障礙物很近了約束卻顯示還沒違反。我的實測經歷是當平滑系數(shù)超過 0.5 時軌跡在狹縫中穿過的概率明顯上升因為約束值被“平均”掉了。建議在需要嚴格防碰撞的場景里不要用平滑聚合而是使用顯式的分離約束。如果一定要用把平滑系數(shù)控制在 0.1 左右并在約束計算時額外補償一個 0.05 米的保守偏移量。4.3 乘子初始化不當導致軌跡全程避險過度AL 乘子是一個很敏感的東西。如果一開始就把乘子設成很大的值優(yōu)化器會認為任何接近障礙物的路徑都是高代價的最后可能給出一個繞遠路甚至倒退行駛的怪異軌跡。我習慣把乘子初始化為 0讓第一輪 iLQR 純粹靠原有代價函數(shù)的“慣性”把軌跡拉向目標后面再用懲罰項修正方向。還有一個常見的錯誤是乘子更新太激進。λ 更新的幅度如果過大軌跡會在某一輪突然被推離障礙物很遠然后在下一輪因為追趕目標又靠近障礙物形成震蕩。解決方式是限制 λ 單步增幅比如最多只允許在上一輪基礎上增加 2 倍超過這個范圍就保持不變等內層迭代自然適應。4.4 可視化調試的必做動作這可能是所有調試技巧里最重要的一條把每個圓的圓心、半徑、距離線段和最近點全部畫出來疊加在軌跡動畫上調試。不要只在結果里看 vehicle footprint in a top-down view。我通常在優(yōu)化循環(huán)的每次外層迭代里都輸出一下該輪軌跡上各個控制點的“圓到障礙最近距離”曲線。如果最近距離曲線在某個時間點突然掉到接近零再彈起來那大概率是貨車轉彎掃掠包絡漏了中間的那個圓而不是算法崩潰。這么畫的另一個好處是能快速定位乘子的作用對象。你會清楚看到乘子到底在“拉”哪個圓的哪個方向而不是對著一個聚合成的最小距離值干瞪眼。AL 方法的最難調教之處就在這種逐條約束的交互上一個收斂性的局部最小值往往能通過再加一個約束、改一個乘子初值就解決掉。4.5 多障礙物稠密場景下的約束裁剪如果地圖里有 20 個障礙物、自車 5 個圓那么理論上要生成 100 條分離約束。iLQR 每次迭代里 100 條約束都求導、都算乘子不僅慢還有大量約束是無效的因為該圓和某個障礙物之間距離非常遠根本不構成威脅。實際項目中我會做一個候選約束篩選先計算每個圓的圓心到每個障礙物中心的距離只保留“圓心距離 圓半徑 障礙外接半徑 剪裁閾值”的約束組合其他的直接丟棄。剪裁閾值一般取 1.0 米這樣既保證安全又把單輪約束數(shù)量壓縮到了一個很穩(wěn)定的水平。剪裁本身不會破壞求解正確性因為被剪掉的約束在真實距離上離安全邊界還很遠乘子變化也幾乎可以忽略。4.6 初始軌跡對 AL-iLQR 的成敗影響最后需要強調的一點也是我反復跟團隊說的AL-iLQR 不是萬能黑盒它對初始軌跡的合理性非常敏感。如果你拿一條完全橫穿障礙物的直線作為起始軌跡增廣拉格朗日極大概率會收斂到某個貼著障礙物繞行的怪異路徑而不是把車優(yōu)雅地拉回車道。一個穩(wěn)妥的做法是先用粗糙的全局路徑規(guī)劃器生成一條符合拓撲的參考路徑再把它離散化成 iLQR 的初始軌跡。AL 乘子在大方向不對時會幫優(yōu)化器找到局部修正但不要指望它能做全局拓撲變換。多圓碰撞約束在這套流程里承擔的是“最后一公里”的精細化修正保證路徑不刮擦、不留安全冗余而不是負責從零找出一條路來。我個人實際操作中最受益的一點是把碰撞約束的復雜度從矩形降到多圓以后很多以前躲不開的調試死角都變得清晰可見了。圓與圓的梯度是獨立的乘子也是獨立的哪個圓在哪個方向上有問題一眼就能定位。這種“可解釋性”是這套方案最大的額外價值。等到后期想更進一步可以考慮把障礙物也拆成圓或者膠囊體讓距離計算變成圓與圓的解析距離那樣梯度更干凈但碰撞精度會略微下降適合與本文方案做個對比驗證看哪種更貼合你的實車表現(xiàn)。