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