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