巨人的肩膀：

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

1

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

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

2

場景選擇階段：ScenarioDispatchNonLearning()函數(shù)默認從lanefollow場景開始判斷，首先根據(jù)駕駛員的意圖來安排場景，如果不是默認的lanefollow場景，直接輸出當(dāng)前場景；如果是lanefollow場景，會依次判斷是否屬于別的場景；即剩余場景之間的跳轉(zhuǎn)必須經(jīng)過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: 停止標(biāo)志case ScenarioConfig::STOP_SIGN_UNPROTECTED:case ScenarioConfig::TRAFFIC_LIGHT_PROTECTED: 信號燈case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN: 信號燈無保護左轉(zhuǎn)case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:case ScenarioConfig::VALET_PARKING: 代客泊車case ScenarioConfig::DEADEND_TURNAROUND: 路端調(diào)頭

3

lane_follow的task列表：

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

它的作用主要有兩點：

判斷當(dāng)前是否進行變道，以及變道的狀態(tài)，并將結(jié)果存在變量lane_change_status中；
變道過程中將目標(biāo)車道的reference line放置到首位，變道結(jié)束后將當(dāng)前新車道的reference line放置到首位

靜態(tài)障礙物的nudge是用的lane_change，至于lane_change和lane_borrow的區(qū)別目前還不懂，之后再補

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

PathReuseDecider

它的作用主要是換道時：

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

Path_Lane_Borrow_Decider

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

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

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

PathBoundsDecider

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

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

PathAssessmentDecider

它的作用主要是：

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

在這里debug時候觀察到，regular/self的路徑被排到了最前面，發(fā)現(xiàn)是因為排序的ComparePathData中設(shè)置的規(guī)則：

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一下發(fā)現(xiàn)regular/self的路徑長度和regular/left的路徑長度竟然一樣，這是因為啥呢，繼續(xù)往前看。
來看看這個path_bound是怎么生成的
->PathBoundsDecider::GenerateRegularPathBound()

直到這時，在這里發(fā)現(xiàn)了apollo就沒給動態(tài)障礙物寫生成借道邊界的邏輯：
看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.

根據(jù)大神的指點開始調(diào)試
改變靜態(tài)障礙物判定邏輯后發(fā)現(xiàn)小車可以變道，但是速度保持在和前車相同
這可以說明PATH_LANE_BORROW_DECIDER給的借道判斷正確

根據(jù)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 是根據(jù)map得到的限制
speed_limit_from_centripetal_acc 路徑曲率帶來的速度限制
speed_limit_from_nearby_obstacles 根據(jù)障礙物得到的速度限制

speed decider 流程圖

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

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

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

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

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

生成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.//將路徑邊界初始化為一個無限大的區(qū)域。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基于靜態(tài)障礙微調(diào)邊界PathBound temp_path_bound = *path_bound;if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),//將不是靜態(tài)的排除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(); }

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

可以看到?jīng)Q策塊也變成了overtake。

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。