planning深度剖析

planning深度剖析

• 結合find命令過濾目錄及文件名后綴: find /home/hadoop/nisj/automationDemand/ -type f -name '*.py'|xargs grep -n 'data_chushou_pay_info'
• 配置文件在每個模塊的common文件的*_gflags.cc文件下定義.
• lattice文件夾下面的代碼應該是路網格子相關的代碼

planner文件夾下的內容

• EM_Planner -- EMPlanner是一個期望最大的planner
  • 在EMPlanner::Plan函數中主要做的事情：
    • 更新planning start point
    • scenario_初始化
    • scenario_處理planning_start_point和frame
• refernce_line -- 參考線相關的函數接口都在這里面,
  • cos_theta問題
  • qp_spline平滑
  • spiral problem平滑
• deciders -- 決策者
• tuning -- 調整

Apollo Planning簡介

• 整體架構:
  • Apollo的定位，行為預測，感知，導航，高精定位都是通過Apollo適配器接入Planning的。
  • planning主要根據上面的信息轉換為參考線提供器，數據處理與管理。
  • 針對參考線做二維參數化的Spline平滑器。
  • 然后接入任務管理器進行任務管理。
  • 然后進入路徑、速度綜合器。
  • 再進入最終結果驗證器。
  • 整合傳統的決策與規划兩個模塊。
  • 統一、靈活、快捷。
  • 參考線與主算法分離。
• 平滑的參考線是決策規划的基礎、數據驅動的要求，采用二維Spline和異步更新，
  \[C(x,y)=C(f_{X}(t),f_{Y}(t)) \]
  • Spline函數。
• 將三維問題轉換為X-Y(路徑規划(x,y))平面和S-T(速度規划(s,t))平面問題來處理。
• 數據設計:
  • 模塊配合復雜，需求主導:
    • 行為預測(上游): proto中增加保護字段。
    • 車輛控制(下游): 分離實際位置與規划位置。
  • 障礙物屬性復雜, 提取共性:
    • S-L邊界(boundary)。
    • 阻塞S-T邊界(boundary)。
    • 橫向、縱向決策。
  • 自身結果復雜:
    • 分而治之， 路徑點、速度點、參考線點、trajectory點。
    • 橫向決策: 避讓、繞行。
    • 縱向決策: 停止、跟車、避讓、超越。
    • boundary有四個點構成((s0, l0), (s1, l1), (s2, l2), (s3, l3))。
• planning還需要考慮:
  • 交通規則。
  • 路徑優化。
  • 路徑決策。
  • 速度優化(DP)。
  • 速度決策。
  • 速度優化(QP)。
• 交通規則:
  • 核心特點： 多。
  • 容易適配、避免沖突、周期一致。
  • 規則工廠，靈活配置不同規則。
  • 規則庫:
    • 后方車輛；
    • 人行橫道；
    • 探頭右拐；
    • 行駛終點；
    • 借道繞行；
    • Stop標志；
  • 橫向和縱向決策分離+決策融合；
  • 有限狀態機: drive --> wait --> side pass --> drive。
• 路徑規划策略:
  • RRT(rapidly exploring random tree)： 隨機取點， 路徑不穩定，常用於機器人走迷宮。
  • State-lattice：格點預先設定，靈活度差，沒有考慮道路特點。
  • Quadratic Programming(二次規划): 成本函數受限， 受地圖中心線影響大, 此問題的解決不受規划模塊控制。
  • 最終的策略: 基於采樣，結合車輛與道路信息，比state-lattice靈活性更強。
  • 離散化求解空間。
    • 使用平滑曲線連接各層的采樣點。
    • 計算各條路徑的成本。
    • 動態規划選擇合適的道路。
  • 道路采樣的優勢:
    • 避免陷入局部最優；
    • 概率完備避免無解；
    • 低速到高速全覆蓋；
    • 道路點采樣 --> 路徑生成 <--> Cost計算 <--> 路勁選擇 --> 最終路徑。
  • 碰撞+物理邊界+虛擬邊界+數值(安全值+舒適值):
    • 層層比較得到一個最終比較數值大小
• 速度規划器 - 動態規划
  • 與主車有重疊的障礙物映射到ST圖中：
    • 停車(stop)；
    • 避讓(yield);
    • 超越(overtake);
  • 限速：ST路的限速區；
  • 把問題轉換為有限求解空間內的最優化問題。
    • 構建網格;
    • 構造成本函數;
      • 安全性相關：與障礙區的間距；
    • 體感相關:
      • 主車速度、加速度；
      • 主車速度變化率；
    • 到達目的地相關:
      • s軸相對位置。
    • 動態規划選擇成本最少的路徑。

解析EM-planning

• em_planner繼承於PlannerWithReferenceLine， PlannerWithReferenceLine繼承於Planner.
• Planner有三個成員變量: PlanningConfig, ScenarioManager, Scenario.
  • PlanningConfig是定義在planning_config.proto中, 由planning_config.pb.txt文件配置。
  • ScenarioManager是一個類，有Init, mutable_scenario, Update三個共有的成員函數和RegisterScenarios, DecideCurrentScenario兩個私有的成員函數。
    • common::util::Factory<ScenarioConfig::ScenarioType, Scenario>
      scenario_factory_工廠成員變量。
    • std::unique_ptr scenario_; 一個Scenario獨享指針。
  • ScenarioManager是一個單例模式, 可以獲得scenario的分析結果, 有兩個私有的成員變量FeatureExtractor和ScenarioAnalyzer。
    • FeatureExtractor是一個為場景分析提供專有特征的類
      • 里面有一個ADCTrajectoryContainer私有類，PoseContainer類和一個ScenarioFeature類。
        • ADCTrajectoryContainer是車輛軌跡的容器, 繼承於Container, 里面有planning::ADCTrajectory(車的軌跡點), common::math::Polygon2d(二維的多邊形, 用於連接(junction)), std::shared_ptr 共享指針用於連接, 一個浮點的連接距離, std::unordered_set std::string一個hashtable的集合用於車道線的indexes， 一個std::vector std::string用於保存車道線的序號， 還有一個std::mutex互斥量用於保存軌跡點。
  • Scenario類中主要有Init、Process和Transfer這三個接口函數。
• PlannerWithReferenceLine繼承Planner的后抽象一個接口PlanOnReferenceLine， 主要有三個參數const common::TrajectoryPoint& planning_init_point, Frame* frame, ReferenceLineInfo* reference_line_info。
  • Frame包含了做一次planning的一幀數據(Frame)。
• EM-planning中的實現代碼其實很少， 就一個Init函數和一個Plan函數；
  • 在Init函數中主要做的事情是獲得PlanningConfig的配置參數， 和初始化場景管理器(scenario_ = scenario_factory_.CreateObject(ScenarioConfig::LANE_FOLLOW)😉 -- 其實就是創建一個LANE_FOLLOW的場景。
  • Plan主要做的事情是scenario_manager更新Frame, 然后獲得scenario, 再初始化這個scenario的一些參數， 最后從開始點處理scenario。
    • ScenarioManager::Update中新建一個場景;
      • DecideCurrentScenario函數中會根據TrajectoryPoint和Frame兩個參數做Transfer(其實就是直接返回ScenarioConfig::LANE_FOLLOW)。
    • mutable_scenario就是直接返回scenario的原始指針，因為是調用的scenario_.get();而scenario_是一個std::unique_ptr 指針, 到最后只有一個指針指向Scenario對象。
    • Scenario::Init是一個純虛函數，就是一個接口， 最終會調用到LaneFollowScenario::Init這個函數；
      • 會注冊一些task, 包括DP的路徑優化器，路徑決策器，DP的速度優化器，速度決策器， QP的速度優化器，多項式的速度優化器。
    • scenario_->Process(planning_start_point, frame);情景的處理
      • 最后其實還是調用LaneFollowScenario::Process， 在Process中調用PlanOnReferenceLine；然后調用各個優化器的Execute函數。
      • 在Process中最后通過SetDrivable函數把reference_line_info設置為可執行的。

邊看邊記之雜談

• ::google::protobuf::Message& message都是利用容器Container或其派生類PoseContainer、ADCTrajectoryContainer等。
• LaneInfo類封裝了一些車道相關的信息， 在hdmap_common.cc和hdmap_common.h中被定義和聲明一些接口。
• 各種類的繼承關系圖。

一些專屬的數據結構

• 什么是ADCTrajectory?
  • ADCTrajectory的字面意思地自動駕駛車的軌跡, 在planning.proto中定義;
    • 主要有總路徑的長度(total_path_length), 單位是米meters;
    • 總路徑的時間(total_path_time)， 單位是seconds;
    • 重復的軌跡點TrajectoryPoint，TrajectoryPoint中主要包含的有(PathPoint，線性的速度v，線性的加速度a，相對時間relative_time(從軌跡的開始的相對時間))。
      • TrajectoryPoint包含PathPoint和一些速度、加速度和時間相關的信息。
      • PathPoint主要包含了一些坐標、曲率、path開始的距離、lane的id, x方向的導數和y方向的導數。
    • 是否有緊急停車EStop;
    • 路徑點PathPoint(沒有速度信息)。
    • 是否重新規划is_replan；
    • 具體的檔位位置GearPosition;
    • planning的決策結果DecisionResult，主要包含了主要決策(MainDecision)、目標決策(ObjectDecisions)和車輛信號(VehicleSignal);
      • MainDecision主要包含： 當前的任務是那個(MainCruise, MainStop, MainEmergencyStop, MainChangeLane, MainMissionComplete, MainNotReady, MainParking)和重復的TargetLane;
        • TargetLane主要包括了lane的id, 起點和終點限制的速度(speed_limit)。
      • ObjectDecisions就是多個ObjectDecision, 每個ObjectDecision中包含了一個字符串的id, 感知目標的id和一個目標的決策類型ObjectDecisionType；
        • ObjectDecisionType主要有幾種類型: 忽略， 停止， 跟隨， 避讓， 超車， 繞行， 從旁邊超過， avoid聽讓；
    • LatencyStats(潛在的狀態): 主要包括了總時間(total_time_ms), 任務的狀態(task_stats)和初始frame的時間init_frame_time_ms;
      • 其中task_stats主要包括了任務的名字和運行時間(time_ms);
    • routing相關的航向角和debug的控制選項;
    • ADCPathPoint(車輛路徑的點) -- 主要包括x,y,z的三個坐標，曲率(curvature)和航向角(heading， 相對於絕對坐標系來說的)。
    • ADCTrajectoryPoint(Deprecated: replaced by apollo.common.TrajectoryPoint), 里面主要是一個些坐標， 速度， 加速度， 曲率， 曲率的變化率， 相對時間， theta值， 從第一個點開始的距離， sl坐標系中的位置。
    • 車輛的信號VehicleSignal， 主要包括(轉向信號TurnSignal), 遠光燈(high_beam), 近光燈(low_beam), 喇叭(horn)和應急燈(emergency_light)。
    • 道路右邊的狀態；
    • 沿車道中線參考線的lane ID;
    • 根據當前的計划結果設置engage的建議; -- 這是一個遇到critical時才給予的運行時建議。
    • 致命區域(CriticalRegion)。
    • 軌跡的類型(TrajectoryType) -- 不知道是什么類型， 正常的類型， 路徑反饋的類型， 速度反饋的類型;
• ScenarioFeature，是在scenario_feature.proto文件重定義的；
  • 主要包括了速度，加速度，航向角， 當前lane的id, lane被走過的距離, 左邊相鄰的laneid和lanes, 右邊相鄰的laneid和lanes, 連接處的距離和id， 障礙物的id。