巨人的肩膀：

Apollo planning 框架
Apollo Planning 代碼學習（一）
apollo介紹之planning模塊
Apollo Planning決策規劃代碼詳細解析 (6):LaneChangeDecider

1

主要的函數流程如下：
PlanningComponent::Proc() -> OnLanePlanning::RunOnce() ->
OnLanePlanning::Plan(）-> PublicRoadPlanner::Plan() ->
Scenario::Process() -> LaneFollowStage::Process() -> LaneFollowStage::PlanOnReferenceLine()

PlanningComponent::Proc()
Proc主要是檢查數據，并且執行注冊好的planning，生成路線并且發布。
OnLanePlanning::RunOnce()
該函數的實現主要分為三個部分:車輛狀態更新、查看全局路徑是否有更新、Planning模塊運行需要時間
OnLanePlanning::Plan(） PublicRoadPlanner::Plan()
依據decider和optimizer進行規劃，調用具體的planner進行規劃，更新場景，決策當前應該執行什么場景，獲取當前場景
Scenario::Process()
執行當前場景的任務
LaneFollowStage::Process()
遍歷所有的參考線，直到找到可用來規劃的參考線后退出
LaneFollowStage::PlanOnReferenceLine()
//順序執行stage中的任務

2

場景選擇階段：ScenarioDispatchNonLearning()函數默認從lanefollow場景開始判斷，首先根據駕駛員的意圖來安排場景，如果不是默認的lanefollow場景，直接輸出當前場景；如果是lanefollow場景，會依次判斷是否屬于別的場景；即剩余場景之間的跳轉必須經過lanefollow這個場景。

switch (current_scenario_->scenario_type()) {case ScenarioConfig::LANE_FOLLOW: 車道保持case ScenarioConfig::PULL_OVER: 靠邊break;case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED: 無保護十字路口case ScenarioConfig::EMERGENCY_PULL_OVER: 緊急靠邊case ScenarioConfig::PARK_AND_GO: 停車和啟動case ScenarioConfig::STOP_SIGN_PROTECTED: 停止標志case ScenarioConfig::STOP_SIGN_UNPROTECTED:case ScenarioConfig::TRAFFIC_LIGHT_PROTECTED: 信號燈case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN: 信號燈無保護左轉case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:case ScenarioConfig::VALET_PARKING: 代客泊車case ScenarioConfig::DEADEND_TURNAROUND: 路端調頭

3

lane_follow的task列表：

stage_type: LANE_FOLLOW_DEFAULT_STAGEenabled: truetask_type: LANE_CHANGE_DECIDER 判斷當前是否進行變道，以及變道的狀態task_type: PATH_REUSE_DECIDER 換道時：根據橫縱向跟蹤偏差，來決策是否需要重新規劃軌跡；如果橫縱向跟蹤偏差，則根據上一時刻的軌跡生成當前周期的軌跡，以盡量保持軌跡的一致性task_type: PATH_LANE_BORROW_DECIDER 只是判斷是否滿足借道條件，具體的軌跡是否借道，是由后面的task決定task_type: PATH_BOUNDS_DECIDER 根據借道信息、道路寬度生成FallbackPathBound根據借道信息、道路寬度生成、障礙物邊界生成PullOverPathBound、LaneChangePathBound、 RegularPathBound中的一種將車道線和障礙物轉為上圖中的邊界task_type: PIECEWISE_JERK_PATH_OPTIMIZER 根據之前decider決策的reference line和path bound,以及橫向約束，將最優路徑求解問題轉化為二次型規劃問題;調用osqp庫求解最優路徑;task_type: PATH_ASSESSMENT_DECIDER 選出之前規劃的備選路徑中排序最靠前的路徑;添加一些必要信息到路徑中task_type: PATH_DECIDER 在上一個任務中獲得了最優的路徑，PathDecider的功能是 根據靜態障礙物做出自車的決策， 對于前方的靜態障礙物是忽略、stop還是nudgetask_type: RULE_BASED_STOP_DECIDER 根據一些規則設定停止標志task_type: ST_BOUNDS_DECIDERtask_type: SPEED_BOUNDS_PRIORI_DECIDER 將規劃路徑上障礙物的st bounds加載到路徑對應的st圖上計算并生成路徑上的限速信息task_type: SPEED_HEURISTIC_OPTIMIZER 使用DP求解一條最優路徑task_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDER#task_type: PIECEWISE_JERK_SPEED_OPTIMIZERtask_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_type: RSS_DECIDER

LANE_CHANGE_DECIDER

它的作用主要有兩點：

判斷當前是否進行變道，以及變道的狀態，并將結果存在變量lane_change_status中；
變道過程中將目標車道的reference line放置到首位，變道結束后將當前新車道的reference line放置到首位

靜態障礙物的nudge是用的lane_change，至于lane_change和lane_borrow的區別目前還不懂，之后再補

補：lane_chage的觸發條件是有一個以上的參考線，而lane_borrow的條件是只能有一條參考線

PathReuseDecider

它的作用主要是換道時：

根據橫縱向跟蹤偏差，來決策是否需要重新規劃軌跡；
如果橫縱向跟蹤偏差，則根據上一時刻的軌跡生成當前周期的軌跡，以盡量保持軌跡的一致性

Path_Lane_Borrow_Decider

需要進入借道場景才可以觸發繞行功能。

需要滿足下面條件才能判斷是否可以借道：

○ 只有一條參考線，才能借道
○ 起點速度小于最大借道允許速度
○ 阻塞障礙物必須遠離路口
○ 阻塞障礙物會一直存在
○ 阻塞障礙物與終點位置滿足要求
○ 為可側面通過的障礙物

PathBoundsDecider

分四種情況對pathBound進行計算，按照處理順序分別是fallback備用、pullover靠邊停車、lanechange換道、regular常規，不同的boundary對應不同的應用場景，
其中fallback對應的path bound一定會生成，其余3個只有一個被激活，即按照順序一旦有有效的boundary生成，就結束該task。

對于決策規劃–超車任務來說，這里會生成三種path bound ：fallback、regular/self、regular/left/forward

PathAssessmentDecider

它的作用主要是：

○ 選出之前規劃的備選路徑中排序最靠前的路徑；
○ 添加一些必要信息到路徑中。

在這里debug時候觀察到，regular/self的路徑被排到了最前面，發現是因為排序的ComparePathData中設置的規則：

if (lhs_on_selflane || rhs_on_selflane) {if (std::fabs(lhs_path_length - rhs_path_length) >kSelfPathLengthComparisonTolerance) {return lhs_path_length > rhs_path_length;} else {return lhs_on_selflane;}

log一下發現regular/self的路徑長度和regular/left的路徑長度竟然一樣，這是因為啥呢，繼續往前看。
來看看這個path_bound是怎么生成的
->PathBoundsDecider::GenerateRegularPathBound()

直到這時，在這里發現了apollo就沒給動態障礙物寫生成借道邊界的邏輯：
看issue里面怎么說：

https://github.com/ApolloAuto/apollo/issues/8808 Hi, Thanks for the question. The current path-decision can take care of slow-speed obstacles. Here are a few parameters that you can tune to achieve your goal:path_decider_obstacle_utils.cc line 40: Here you can modify it by deleting the "IsStatic()" check and increase the FLAGS_static_obstacle_speed_threhold to be the slow-speed threshold you want. The ADC will try to side-pass any obstacle below this given speed.path_lane_borrow_decider.cc line 103: The FLAGS_lane_borrow_max_speed is the speed for the ADC to start side-pass. If you want the ADC to be able to start side-pass at a faster speed, you can increase this value (at the cost of reduced safety so please be careful).path_lane_borrow_decider.cc line 110: Only when the ADC thinks the blocking obstacle is a long-term one, it will start side-pass. Here you can specify how many frames an obstacle is blocking can it be defined as a "long-term" blocking obstacle. For example, if you specify it to be 10, then only when the ADC sees a slow-car ahead for 10frames (1sec) will the ADC start to pass the slow-car.path_bounds_decider.cc line 270: Here you see that the boundary is determined by lane-width, if you want, you can replace it with the function GetBoundaryFromRoads, what that will do is to use the entire road (likely consisting multiple lanes) as the boundary, ADC will try to use any path that it can find on the road, without considering whether it needs to borrow neighbor lane or not, etc. Normally, we don't suggest use this because it might create some safety issues. But as you can see, at line 319, when the ADC needs to pull-over, especially in emergency situation, it will use this method to find a path.Hope this answers your questions. Thanks.

根據大神的指點開始調試
改變靜態障礙物判定邏輯后發現小車可以變道，但是速度保持在和前車相同
這可以說明PATH_LANE_BORROW_DECIDER給的借道判斷正確

根據SPEED_BOUNDS_PRIORI_DECIDER模塊，我們先來看看是否受到了限速

curr_speed_limit = std::fmax(speed_bounds_config_.lowest_speed(),std::min({speed_limit_from_reference_line,speed_limit_from_centripetal_acc,speed_limit_from_nearby_obstacles}));

speed_limit_from_reference_line 是根據map得到的限制
speed_limit_from_centripetal_acc 路徑曲率帶來的速度限制
speed_limit_from_nearby_obstacles 根據障礙物得到的速度限制

speed decider 流程圖

注：超車時，在ST圖中障礙物上方采樣，即自車速度大于障礙物車速度。

根據log，得到AINFO:ADC在ST圖上位于障礙物下方。但是想overtake需要在障礙物上方。如圖：

查看GetSTLocation()函數，該函數是根據路徑決策、速度曲線、障礙物ST邊界獲取到STLocation。
動態障礙物的ST邊界應該是平行四邊形
先看看輸入的路徑決策、速度曲線分別為
reference_line_info->path_decision()
reference_line_info->speed_data()
回到上一個SPEED_BOUNDS_PRIORI_DECIDER，觀察到給的speed_limit并不小。
判斷還是路徑規劃有問題，主要看PATH_BOUNDS_DECIDER：

Task初始化，根據參考線建立frenet坐標系，計算自車在frenet坐標系中的位置(s,l,s’,l’),同時計算自車到車道中心線的距離以及左右車道寬度。

接下來生成path bound，每個path bound都會在后續生成一條軌跡。

生成RegularPathBound：

Status PathBoundsDecider::GenerateRegularPathBound(const ReferenceLineInfo& reference_line_info,const LaneBorrowInfo& lane_borrow_info, PathBound* const path_bound,std::string* const blocking_obstacle_id,std::string* const borrow_lane_type) {// 1. Initialize the path boundaries to be an indefinitely large area.//將路徑邊界初始化為一個無限大的區域。if (!InitPathBoundary(reference_line_info, path_bound)) {const std::string msg = "Failed to initialize path boundaries.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// PathBoundsDebugString(*path_bound);// 2. Decide a rough boundary based on lane info and ADC's position//基于道路信息和車輛生成粗略邊界if (!GetBoundaryFromLanesAndADC(reference_line_info, lane_borrow_info, 0.1,path_bound, borrow_lane_type)) {const std::string msg ="Failed to decide a rough boundary based on ""road information.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// PathBoundsDebugString(*path_bound);// TODO(jiacheng): once ready, limit the path boundary based on the// actual road boundary to avoid getting off-road.// 3. Fine-tune the boundary based on static obstacles基于靜態障礙微調邊界PathBound temp_path_bound = *path_bound;if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),//將不是靜態的排除path_bound, blocking_obstacle_id)) {const std::string msg ="Failed to decide fine tune the boundaries after ""taking into consideration all static obstacles.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// Append some extra path bound points to avoid zero-length path data.int counter = 0;while (!blocking_obstacle_id->empty() &&path_bound->size() < temp_path_bound.size() &&counter < kNumExtraTailBoundPoint) {path_bound->push_back(temp_path_bound[path_bound->size()]);counter++;}// PathBoundsDebugString(*path_bound);// 4. Adjust the boundary considering dynamic obstacles// TODO(all): may need to implement this in the future.ADEBUG << "Completed generating path boundaries.";return Status::OK(); }

根據車輛和障礙物以及道路信息生成bound，即下圖：
由于障礙物是非靜態的，所以障礙物的end_s會有一個延長，所以會造成s-ref被堵塞，故進行buffer的調整。
成功：

可以看到決策塊也變成了overtake。

如果對您有參考價值。希望不吝點贊：）。

總結

以上是生活随笔為你收集整理的apollo决策规划学习--慢速障碍物超车的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。