規劃是門很深的學問，也是自動駕駛的核心功能，所以很難通過一兩篇文字講清楚，所以我也會持續更新，今天要講的是Apollo的規劃方式，網上有一篇ppt的介紹和我自己對源碼的分析，總結出這樣一篇文章，原計劃是去年年底把我的博客衝到5000以內，但是去年公司發生一些事受到影響，還好年後又趨於穩定，但是不免又懈怠了下來，不過始終不忘初心，對於提高自己自動駕駛的能力念念不忘，相信博客還會持續更新。
很多人質疑過：寫博客就能提高自動駕駛能力嗎？理論和實踐還差的很遠呢。我對這件事的理解是：理論實踐相結合是最高目標，所謂“知行合一”，爲此陽明先生親口做出過解釋：“知爲行之始，行爲知之成”。寫博客能解決知的問題，至於行就需要日常工作生活中不斷提高對自己的要求，切勿眼高手低、想一蹴而就，依靠長期的修行自然水到渠成。另外頭條也會分享一些我的生活和感悟，相信以後自己回看也很有味道吧。
1. 規劃的輸入輸出
規劃模塊以預測模塊、routing模塊、高精地圖和定位的結果作爲輸入，通過算法，輸出一條平穩、舒適、安全的軌跡，交給控制模塊去執行。一個合格規劃算法，必須滿足幾個條件：
- 1. 必須能夠使自動駕駛汽車到達目的地；
- 2. 必須符合交規；
- 3. 能夠避免碰撞；
- 4. 也需要能保證一定的舒適性。
2. 工作流程
1. 對於任何一個場景，都有無數的軌跡應對當前的場景，有的激進，有的保守，那麼Lattice規劃算法的第一步就是採樣足夠多的軌跡，提供儘可能多的選擇。
2. 第二步是計算每一條軌跡計算的cost。這個cost考慮了軌跡的可行性、安全性等因素。
3. 第三步就是一個循環檢測的過程。在這個過程中，我們每次會先挑選出cost最低的軌跡，對其進行物理限制檢測和碰撞檢測。如果挑出來的軌跡不能同時通過這兩個檢測，就將其篩除，考察下一條cost最低的軌跡。
4. 假設我們現在挑選出軌跡，它既符合汽車的物理性狀，也不會有碰撞風險，就將這條軌跡作爲規劃軌跡輸出。
3. 採樣過程
1. 首先我們可以通過計算得到自動駕駛汽車在Frenet座標系下的在零時刻的起始狀態，也就是汽車的當前狀態。爲了生成一條軌跡，第一步就是在Frenet座標系下采樣一個在T1時刻的末狀態。
2. 第二步就是將末狀態和起始狀態做多項式擬合。分別形成橫向和縱向的多項式軌跡。
3. 第三步就是二維合成。給定一個時刻T*，我們可以計算出在T*時刻的縱向偏移量和橫向偏移量，再通過參考線，即可還原成一個二維平面中的軌跡點。
4 .計算Cost
我們前面提到，軌跡規劃所需要滿足的四點要求，分別是到達目的、符合交規，避免碰撞、平穩舒適。針對這四點要求，我們設計了六個cost，cost越高就表示越不滿足要求。
1. 首先是到達目的的cost。這裏分成兩種情況，一個是存在停車指令（比如紅燈）的情況，另一個是沒有停車指令的。如果存在停車指令，相對大的車速，其對應的軌跡cost就越大；如果沒有停車指令，那麼低速軌跡的cost就會越大。
2. 第二個cost是橫向偏移cost。設計這個cost是爲了讓自動駕駛汽車能儘量沿着道路中心行駛。那麼像左圖汽車靠道路一邊行駛，和中圖畫龍的行駛軌跡，他們的cost都相對較高。
3. 第三個cost是碰撞cost。有碰撞風險，那麼它的碰撞cost就會相對較高。
4. 第四個cost是縱向加加速度的cost。加加速度（jerk）是加速度對時間的導數，表示加速度的變化率。我們用加加速度的最大值值來表示這個cost。
5. 第五個cost是橫向加速度的cost。設計這個cost是爲了平穩地換道。那麼像左圖猛打方向盤的軌跡，它的橫向加速度cost就會相對較大。
6. 最後一個cost是向心加速度cost。設計這個cost是爲了在轉彎或調頭的時候能夠減速慢行。在彎道處，車速慢的軌跡，其向心加速度cost就會相對較低，那麼就會更容易被率先挑選出來。
這六個cost的加權求和就是軌跡的總cost。開發者可以根據產品的需要，調試這六個權重。
對於換道場景，Lattice算法僅僅需要對目標車道對應的參考線做一次採樣+選擇的流程。本車道和目標車道均能產生一條最優軌跡。給換道軌跡的cost上增加額外的車道優先級的cost，再將兩條軌跡比較，選擇cost較小的那條即可。
5. 代碼簡介
1. Lattice planner程序入口：
Status LatticePlanner::PlanOnReferenceLine(const TrajectoryPoint& planning_init_point, Frame* frame,ReferenceLineInfo* reference_line_info)；
2. 獲得當前參考線（車輛未來軌跡），並通過TodiscretizedReferenceLine進行參考線的離散化.
auto ptr_reference_line = std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine( reference_line_info->reference_line().reference_points()));
3. 使用規劃原點匹配第一步離散化後的軌跡，找出匹配點matched_point。
PathPoint matched_point = PathMatcher::MatchToPath(*ptr_reference_line, planning_init_point.path_point().x(), planning_init_point.path_point().y());
4.使用ComputeInitFrenetState計算當前狀態下，Frenet座標系下的點，S和D值，橫向速度，縱向速度等。
ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);
5.加入感知信息，將之前感知模塊內容加入到path_time_grapth，也就是所謂的ST-graph
auto ptr_prediction_querier = std::make_shared<PredictionQuerier>( frame->obstacles(), ptr_reference_line);
加入感知信息時，裏面涉及多個函數，具體包括障礙物篩選，去掉與車輛未來軌跡不發生衝突的障礙物，設置動態障礙物。
在這裏還有一個planning_target判斷，即判斷是否有停車點，如果有的話打印出停車點的s值（Frenet座標系下的縱向值）.
6. 這一步很關鍵，該步是生成橫縱向軌跡：
Trajectory1dGenerator trajectory1d_generator(init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);
std::vector<std::shared_ptr<Curve1d>> lon_trajectory1d_bundle;
std::vector<std::shared_ptr<Curve1d>> lat_trajectory1d_bundle;
trajectory1d_generator.GenerateTrajectoryBundles(planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);
涉及的函數很多，具體包括縱向軌跡生成以及橫向軌跡生成，通過設置初始狀態、末狀態，通過五次多項式擬合，得出相關係數，再通過求導，即可得出速度，再求導得出加速度。再求導得出的加加速度，也就是Jerk，這是用來評價軌跡的舒適度的變量。
7. 橫向控制是根據縱向控制來進行採樣 其中橫向位移爲{0，-0.5,0.5}、縱向位移爲{10,20,40,80}分別採樣，最後得到多組橫向的軌跡，同樣也是使用5次多項式擬合。注意：橫向的軌跡是以縱向位移s爲自變量.
void Trajectory1dGenerator::GenerateLateralTrajectoryBundle();
8.軌跡評價函數 上一步生成了許多條軌跡，這裏就需要選擇出cost最小的一條軌跡作爲未來軌跡，
TrajectoryEvaluator trajectory_evaluator(init_s, planning_target, lon_trajectory1d_bundle, lat_trajectory1d_bundle, ptr_path_time_graph, ptr_reference_line);
Evaluate評估的時候有5個cost值，分別是
- cost of missing the obkective ;
- cost of logitudinal jerk
- cost of logitudinal collision
- cost of lateral offsets
- cost of laterlal comfort
接下來對候選的軌跡集按照cost大小排序，最終選擇cost最小且無碰撞的軌跡
9. 最後生成軌跡，通過下列函數，進行軌跡賦值
reference_line_info->SetTrajectory(combined_trajectory);