1. 從一次詭異的橫向偏移說起StBoundsDecider到底在管什么如果你在Apollo 6.0的規(guī)劃模塊里調(diào)過車大概率遇到過這種場景車輛在直道上跑得好好的一進(jìn)彎道或者旁邊有車壓線方向盤就開始小幅畫龍橫向軌跡像喝多了似的左右微調(diào)。你翻遍LateralController的參數(shù)調(diào)來調(diào)去效果都不理想最后定位到問題出在規(guī)劃輸出的邊界上——具體說就是StBoundsDecider這個任務(wù)給出的橫向可行區(qū)間不合理。StBoundsDecider是Apollo規(guī)劃器PublicRoadPlanner里一個不起眼但極其關(guān)鍵的任務(wù)。它掛在LANE_FOLLOW場景的ST_BOUNDS_DECIDER階段核心職責(zé)是基于參考線和靜態(tài)/動態(tài)障礙物為后續(xù)的速度規(guī)劃器SpeedDecider、DP速度優(yōu)化生成一條隨時間變化的橫向邊界走廊。注意它輸出的是STBoundary也就是時間-橫向位置空間里的約束而不是直接給軌跡。速度規(guī)劃器會在這個走廊里找一條既安全又舒適的速度曲線。很多人第一次看這個模塊會懵為什么橫向邊界要用ST圖來表達(dá)直接給個左右車道線不就行了這里的關(guān)鍵在于動態(tài)障礙物的橫向侵入是隨時間變化的。比如旁邊車道有輛車正在往你這邊并線它對你的橫向擠壓是漸進(jìn)的不是一瞬間占滿。StBoundsDecider要做的就是把這個漸進(jìn)擠壓過程量化成一條隨時間演化的橫向邊界讓速度規(guī)劃器知道在t時刻我的橫向活動范圍被壓縮到了多少。這篇文章我會把Apollo 6.0里StBoundsDecider的完整流程拆開講透包括它的輸入輸出、核心算法邏輯、代碼里幾個容易看漏的細(xì)節(jié)以及我在實際調(diào)試中踩過的坑。適合正在做自動駕駛規(guī)劃模塊開發(fā)、需要深入理解Apollo規(guī)劃架構(gòu)的工程師也適合想搞明白邊界決策這個中間層到底在干什么的讀者。代碼基于Apollo 6.0的modules/planning/tasks/deciders/st_bounds_decider/目錄我會盡量把關(guān)鍵函數(shù)和數(shù)據(jù)結(jié)構(gòu)對應(yīng)上。2. StBoundsDecider在規(guī)劃流水線里的位置與數(shù)據(jù)流2.1 它上游吃什么、下游吐什么先理清楚數(shù)據(jù)流不然看代碼容易迷路。StBoundsDecider的輸入主要來自三塊參考線信息ReferenceLineInfo提供道路的左右邊界、車道寬度、以及參考線上每個點的橫向范圍。這是靜態(tài)邊界的基礎(chǔ)。障礙物列表Obstacles包括靜態(tài)障礙物和動態(tài)障礙物每個障礙物有預(yù)測軌跡Trajectory包含未來一段時間的位置、速度、加速度。規(guī)劃起始狀態(tài)PlanningStartPoint當(dāng)前車輛的位置、速度、朝向用來確定ST圖的起點。輸出是一個StBoundsDecider內(nèi)部維護(hù)的std::vectorSTBoundary最終會塞進(jìn)ReferenceLineInfo的st_graph_data里供下游的SpeedDecider和DpStSpeedOptimizer使用。這里有個容易忽略的點StBoundsDecider輸出的邊界是在Frenet坐標(biāo)系下的橫向邊界也就是l方向橫向偏移的上下限隨時間t變化。它不直接管縱向s縱向的決策是SpeedDecider的事。兩者配合才構(gòu)成完整的ST空間約束。2.2 為什么需要獨立的StBoundsDecider而不是直接用車道的邊界這是我在面試新人時最愛問的問題。答案藏在動態(tài)兩個字里。車道線給出的橫向邊界是靜態(tài)的、幾何的但實際行駛中橫向可行空間會被動態(tài)障礙物實時壓縮。舉個例子你以60km/h行駛右前方一輛車以50km/h同向行駛且略微壓線它對你的橫向擠壓在ST圖上表現(xiàn)為一條斜線——隨著時間推移它離你越來越近你的右邊界不斷左移。如果只用車道邊界速度規(guī)劃器會認(rèn)為右側(cè)一直有足夠空間可能規(guī)劃出一條貼著右車道線行駛的速度曲線結(jié)果就是不斷逼近那輛車最后觸發(fā)急剎或橫向避讓。StBoundsDecider的價值就在于提前把這種動態(tài)壓縮量化出來讓速度規(guī)劃器在時間維度上就知道再過2秒右側(cè)就沒空間了從而提前減速或調(diào)整。2.3 任務(wù)在LANE_FOLLOW場景中的執(zhí)行順序在LANE_FOLLOW場景的Stage配置里ST_BOUNDS_DECIDER排在LANE_FOLLOW_DECIDER之后、SPEED_DECIDER之前。這個順序不是隨便定的LANE_FOLLOW_DECIDER先確定當(dāng)前是跟車、超車還是保持車道輸出橫向決策意圖。ST_BOUNDS_DECIDER根據(jù)這個意圖和障礙物生成橫向邊界走廊。SPEED_DECIDER在走廊內(nèi)做縱向速度決策。DP_ST_SPEED_OPTIMIZER做最終的ST圖速度優(yōu)化。所以StBoundsDecider是承上啟下的角色它把橫向決策意圖翻譯成可量化的邊界約束供縱向優(yōu)化使用。理解這個定位后面看代碼就不會覺得它在多此一舉。3. 核心算法拆解從障礙物到ST邊界的三步走3.1 第一步構(gòu)建初始的ST邊界框架代碼入口在StBoundsDecider::Process()它調(diào)用了InitSTBoundsDecider()。這一步做的是初始化工作核心是確定ST圖的時間范圍和橫向范圍。時間范圍由FLAGS_st_bounds_decider_time_horizon控制默認(rèn)是7秒左右。為什么是7秒這是Apollo在高速場景下的經(jīng)驗值7秒足夠覆蓋大多數(shù)跟車和并線決策的預(yù)判窗口再長預(yù)測精度下降再短來不及做平滑減速。橫向范圍則來自參考線的GetLaneWidth()和當(dāng)前車輛的橫向位置。初始化時會創(chuàng)建一個STBounds對象內(nèi)部用std::vectorstd::pairdouble, double存儲每個時間采樣點的橫向上下限。采樣間隔默認(rèn)0.1秒7秒就是70個點。這個密度是為了保證邊界足夠平滑避免速度規(guī)劃器在離散點上產(chǎn)生抖動。注意時間采樣間隔不要隨意調(diào)大。我試過改成0.2秒結(jié)果在障礙物快速逼近的場景下邊界出現(xiàn)階梯狀跳變速度曲線跟著抖。0.1秒是經(jīng)過驗證的平衡點。3.2 第二步靜態(tài)障礙物與車道邊界的融合靜態(tài)障礙物比如路邊停的車、錐桶對橫向邊界的約束是硬的——它們不隨時間移動所以在ST圖上表現(xiàn)為一條豎直的線在某個時間點之后橫向范圍被永久壓縮。代碼里處理靜態(tài)障礙物的邏輯在GetStaticObstacleSTBoundary()。它會遍歷所有靜態(tài)障礙物對每個障礙物計算它在參考線橫向上的投影范圍然后更新STBounds的上下限。這里有個細(xì)節(jié)不是簡單取交集而是要考慮障礙物的膨脹buffer。Apollo默認(rèn)給靜態(tài)障礙物加0.3米的橫向buffer防止規(guī)劃出的軌跡貼著障礙物邊緣走。車道邊界則來自參考線的GetRoadLeftBound()和GetRoadRightBound()它們本身就是橫向的硬約束。靜態(tài)障礙物的邊界和車道邊界取交集得到靜態(tài)可行域。3.3 第三步動態(tài)障礙物的ST邊界生成這是整個模塊最復(fù)雜的部分代碼在GetDynamicObstacleSTBoundary()。動態(tài)障礙物的處理邏輯可以概括為篩選相關(guān)障礙物不是所有障礙物都影響橫向邊界。只有那些在預(yù)測時間內(nèi)可能進(jìn)入自車橫向范圍的障礙物才需要處理。篩選條件是障礙物的橫向距離小于某個閾值默認(rèn)是車道寬度的一半加buffer。計算障礙物的橫向侵入曲線對每個相關(guān)障礙物根據(jù)它的預(yù)測軌跡計算它在每個時間點占據(jù)的橫向范圍。這里要用到障礙物的預(yù)測軌跡和自車參考線的Frenet轉(zhuǎn)換。更新STBounds把障礙物的橫向侵入范圍從當(dāng)前可行域中挖掉。如果障礙物從右側(cè)侵入就抬高下界從左側(cè)侵入就壓低上界。這里的關(guān)鍵難點是障礙物預(yù)測軌跡的不確定性。Apollo 6.0的預(yù)測模塊會給出多條預(yù)測軌跡及概率StBoundsDecider默認(rèn)取概率最高的那條但會加一個額外的安全buffer。我在實際項目里試過用多軌跡加權(quán)效果提升有限但計算量翻倍后來還是回到單軌跡加buffer的方案。3.4 邊界平滑與合法性檢查生成完原始邊界后代碼會做兩件事平滑和檢查。平滑用的是滑動平均濾波窗口大小3個采樣點。目的是消除障礙物預(yù)測軌跡的抖動導(dǎo)致的邊界毛刺。但平滑有個副作用可能把一些尖銳但必要的約束磨平。比如障礙物突然切入的場景平滑后邊界會提前收縮導(dǎo)致自車過早減速。Apollo的處理方式是只對邊界的變化率做限制而不是直接平滑位置這樣既保證了平滑性又不會丟失關(guān)鍵約束。合法性檢查在IsValidSTBounds()里主要檢查三點上下界不能交叉否則可行域為空、邊界變化率不能超過車輛最大橫向加速度對應(yīng)的值、邊界不能超出道路物理邊界。如果檢查失敗會觸發(fā)一個fallback退回到只用車道邊界的保守方案。4. 代碼級細(xì)節(jié)幾個容易看漏但很關(guān)鍵的地方4.1 STBounds的數(shù)據(jù)結(jié)構(gòu)與更新邏輯STBounds類定義在st_bounds_decider.h里核心成員是std::vectorstd::pairdouble, double st_bounds_; // (lower, upper) per time step std::vectordouble time_steps_;更新邏輯在UpdateSTBounds()里它接收一個障礙物的橫向范圍和時間范圍然后逐時間點更新。注意它是原地更新不是重新計算所以障礙物的處理順序會影響最終結(jié)果。Apollo的做法是先處理靜態(tài)障礙物再按障礙物距離自車的縱向距離從近到遠(yuǎn)處理動態(tài)障礙物。這個順序保證了近處障礙物的約束優(yōu)先符合先保證不撞近的的直覺。4.2 障礙物橫向范圍的Frenet轉(zhuǎn)換動態(tài)障礙物的位置是在笛卡爾坐標(biāo)系下的要轉(zhuǎn)換到參考線的Frenet坐標(biāo)系才能和STBounds比較。轉(zhuǎn)換函數(shù)是cartesian_to_frenet()在modules/common/math/里。這里有個坑轉(zhuǎn)換的精度依賴于參考線的采樣密度。如果參考線采樣太稀疏比如1米一個點轉(zhuǎn)換誤差可能達(dá)到0.1米以上導(dǎo)致邊界不準(zhǔn)。Apollo默認(rèn)參考線采樣是0.5米實測誤差在0.05米以內(nèi)可以接受。4.3 時間對齊問題障礙物的預(yù)測軌跡時間戳和STBounds的時間戳可能不對齊。代碼里用線性插值做對齊插值函數(shù)在STBounds::GetBoundaryAtTime()。插值時要注意如果障礙物預(yù)測軌跡的時間范圍比STBounds短超出部分按障礙物消失處理即不施加約束。這個處理在大多數(shù)場景下沒問題但如果障礙物預(yù)測突然中斷比如預(yù)測模塊超時會導(dǎo)致邊界突然放寬速度規(guī)劃器可能做出激進(jìn)決策。Apollo的緩解措施是給預(yù)測中斷的障礙物加一個虛擬延續(xù)用最后已知速度外推一小段時間。4.4 參數(shù)配置與調(diào)參經(jīng)驗StBoundsDecider相關(guān)的參數(shù)在modules/planning/conf/planning.conf里常用的幾個參數(shù)名默認(rèn)值作用調(diào)參建議st_bounds_decider_time_horizon7.0ST圖時間范圍高速可加到8城區(qū)保持7st_bounds_decider_time_resolution0.1時間采樣間隔不建議改static_obstacle_buffer0.3靜態(tài)障礙物橫向buffer窄路可減到0.2dynamic_obstacle_buffer0.5動態(tài)障礙物橫向buffer保守場景可加到0.8max_lateral_acceleration2.0邊界變化率上限舒適性優(yōu)先可減到1.5我調(diào)過最坑的一個參數(shù)是dynamic_obstacle_buffer。默認(rèn)0.5米在大多數(shù)場景夠用但在國內(nèi)這種電動車亂竄的路況下0.5米經(jīng)常不夠?qū)е逻吔缡湛s太慢速度規(guī)劃器反應(yīng)不及。后來加到0.8米急剎次數(shù)明顯減少但代價是通行效率下降容易被加塞。最后折中到0.6米配合預(yù)測模塊的激進(jìn)程度調(diào)整才找到平衡。5. 實測中的典型問題與排查鏈路5.1 問題一彎道邊界抖動導(dǎo)致方向盤畫龍現(xiàn)象車輛在彎道行駛時橫向軌跡出現(xiàn)周期性小幅振蕩頻率大概2-3Hz。排查過程先看StBoundsDecider輸出的邊界日志發(fā)現(xiàn)邊界在彎道處有高頻抖動。進(jìn)一步看障礙物列表發(fā)現(xiàn)是路邊靜態(tài)障礙物護(hù)欄的投影在彎道處因為參考線曲率變化橫向范圍計算出現(xiàn)跳變。根因靜態(tài)障礙物的橫向投影用的是參考線最近點彎道處最近點切換導(dǎo)致投影范圍突變。修復(fù)在GetStaticObstacleSTBoundary()里加了一個投影平滑對連續(xù)時間點的投影范圍做限幅變化率不超過0.1米/0.1秒。改完后抖動消失。5.2 問題二并線場景下邊界收縮過晚現(xiàn)象旁邊車道車輛并線切入時自車直到很近才開始減速乘坐體驗差。排查過程看預(yù)測模塊輸出的并線車輛軌跡發(fā)現(xiàn)預(yù)測的并線開始時間比實際晚0.5秒左右。StBoundsDecider用的是預(yù)測軌跡所以邊界收縮也跟著晚。根因預(yù)測模塊的并線檢測有延遲這是預(yù)測的固有難題。緩解在StBoundsDecider里加了一個并線意圖預(yù)判如果障礙物的橫向速度朝向自車且超過閾值0.3m/s即使預(yù)測軌跡還沒顯示并線也提前施加一個軟約束。這個改動讓邊界提前0.3-0.5秒收縮體驗改善明顯。代碼上是在GetDynamicObstacleSTBoundary()里加了一段橫向速度檢查。5.3 問題三STBounds為空導(dǎo)致規(guī)劃失敗現(xiàn)象極窄路段兩車交會時規(guī)劃模塊報no valid ST bounds車輛急停。排查過程日志顯示上下界交叉可行域為空。原因是兩車交會時左側(cè)障礙物和右側(cè)障礙物的橫向約束疊加加上buffer后沒有剩余空間。根因buffer設(shè)置過于保守且沒有考慮錯時通過的可能性。修復(fù)在IsValidSTBounds()失敗時不是直接fallback到車道邊界而是嘗試放寬時間維度允許在某個時間點之后才滿足約束即把硬約束變成軟約束時間窗。具體實現(xiàn)是給STBounds加一個relaxation_window在這個窗口內(nèi)允許上下界短暫交叉由速度規(guī)劃器決定是等待還是通過。這個改動比較復(fù)雜但解決了窄路交會的死鎖問題。5.4 排查工具與方法論調(diào)試StBoundsDecider最有效的工具是Apollo自帶的planning_visualizer它能把STBounds畫在ST圖上直觀看到邊界隨時間的演化。我習(xí)慣在每次改動后導(dǎo)出ST圖截圖對比邊界形狀。另外--v3的日志級別會打印每個障礙物對邊界的貢獻(xiàn)定位是哪個障礙物導(dǎo)致邊界異常非常有用。排查思路總結(jié)成一句話先看邊界形狀再看是哪個障礙物貢獻(xiàn)的最后看這個障礙物的預(yù)測軌跡是否合理。大部分問題出在預(yù)測軌跡上StBoundsDecider本身只是忠實地把預(yù)測翻譯成邊界。6. 從StBoundsDecider延伸出的設(shè)計思考6.1 邊界決策與軌跡決策的邊界在哪里StBoundsDecider只給邊界不給軌跡這個設(shè)計哲學(xué)值得琢磨。它的好處是解耦橫向邊界由障礙物決定縱向速度由優(yōu)化器決定兩者互不干擾。但壞處是信息損失邊界只告訴優(yōu)化器哪里不能去沒告訴它哪里更好去。比如邊界內(nèi)有一塊區(qū)域雖然可行但靠近障礙物優(yōu)化器可能因為代價函數(shù)設(shè)計不當(dāng)而選擇貼邊行駛。Apollo的解法是在速度優(yōu)化器的代價函數(shù)里加橫向舒適性項間接引導(dǎo)車輛走中間。但這是間接的不如直接在邊界里加軟約束來得直接。我在自己的項目里試過在STBounds里加一個推薦橫向位置的字段優(yōu)化器參考它做決策效果有提升但增加了耦合度后來還是回歸了純邊界方案。6.2 預(yù)測不確定性如何傳導(dǎo)到邊界這是StBoundsDecider最脆弱的一環(huán)。預(yù)測模塊給的是點估計StBoundsDecider把它當(dāng)確定值用但實際預(yù)測有誤差。誤差傳導(dǎo)到邊界再傳導(dǎo)到速度規(guī)劃最后表現(xiàn)為車輛的保守或激進(jìn)。Apollo 6.0的處理是加buffer但buffer是靜態(tài)的不能隨預(yù)測置信度動態(tài)調(diào)整。一個更優(yōu)雅的方案是用預(yù)測的概率分布生成概率邊界比如取90%分位數(shù)的橫向范圍作為邊界這樣置信度低的障礙物自動獲得更大的buffer。我在實驗里實現(xiàn)過簡化版用預(yù)測軌跡的方差估計分位數(shù)效果不錯但計算量增加約15%。如果你們的平臺算力充裕值得一試。6.3 多障礙物交互時的邊界組合當(dāng)多個障礙物同時影響橫向邊界時StBoundsDecider是逐個處理的取所有約束的交集。這在大多數(shù)場景下沒問題但有一種情況會出問題兩個障礙物分別從左右擠壓單獨看每個都不致命但組合起來可行域為空。前面5.3節(jié)提到的窄路交會就是這種情況。更聰明的做法是考慮障礙物之間的相對關(guān)系如果兩個障礙物是相向而行它們的擠壓是暫時的可以錯時通過如果是同向而行擠壓是持續(xù)的必須找空間。Apollo 6.0沒有做這個區(qū)分這是可以改進(jìn)的點。我在自己的項目里加了一個簡單的障礙物對分析對相向障礙物放寬時間約束效果立竿見影。6.4 參數(shù)自適應(yīng)固定buffer的局限性固定buffer最大的問題是不能適應(yīng)不同場景。高速上0.5米buffer足夠城區(qū)窄路0.5米可能直接導(dǎo)致可行域為空。我試過用場景識別來動態(tài)調(diào)buffer高速用0.5城區(qū)用0.3停車場用0.2。實現(xiàn)方式是在StBoundsDecider初始化時根據(jù)ReferenceLineInfo的道路類型和當(dāng)前速度查表。這個改動不大但減少了大量極端場景下的規(guī)劃失敗。最后分享一個我在實際調(diào)試中的體會StBoundsDecider的問題十有八九不是它自己的問題而是上游預(yù)測或下游優(yōu)化的鍋。它就像一個老實巴交的翻譯把預(yù)測的話原樣轉(zhuǎn)達(dá)給優(yōu)化如果翻譯出來的邊界不合理先別急著改它去看看預(yù)測說了什么。我踩過的坑里至少七成最后定位到預(yù)測模塊的軌跡異常。所以調(diào)試這個模塊先信邊界再疑預(yù)測最后才動代碼這個順序能幫你省下大量時間。