apollo决策规划学习--慢速障碍物超车
巨人的肩膀:
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_DECIDERLANE_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里面怎么說:
根據大神的指點開始調試
改變靜態障礙物判定邏輯后發現小車可以變道,但是速度保持在和前車相同
這可以說明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决策规划学习--慢速障碍物超车的全部內容,希望文章能夠幫你解決所遇到的問題。
- 上一篇: 纽约理工计算机科学怎么样,纽约大学计算机
- 下一篇: 计算机音乐锦鲤抄,锦鲤抄 MIDI Fi