如何生成可行駛路徑
1. 介紹
這次總結一下做uda term3第一個項目，Path planning project時候學習到的內容。
這個項目的目的是讓自動駕駛的車輛在考慮周邊車輛的情況下，如何儘快的在高速上前進。
已知條件：
- 地圖
- cartesian坐標到frenet坐標系變換關係
- 周邊車輛的相對位置
- 動力學用Kinetic模型
項目的repo如下
Fred159/CarND-Term3-Path-Planning-Project
裡面有個pdf是我提交的報告。可以參考一下裡面的內容。關於項目的大部分內容都在PDF裡面。鏈接如下。
https://github.com/Fred159/CarND-Term3-Path-Planning-Project/blob/master/Self_Driving_Car_Term3_Path_Planning_Project.pdf
如果有人想在模擬器上跑這個代碼，可以根據markdown上的simulator下載鏈接進去下載simulator。然後根據md介紹，跑起來就可以了。
2. 生成可行駛路徑
我以前以為，想生成可行駛的路徑，只需要生成一個曲線就好了。
但是，TOO 天真。
為了生成可行駛的路徑，涉及的遠不止是曲線擬合。曲線擬合只是其中的一小部分。為了生成可行駛路徑，起碼要有以下幾點。
Search，Prediction， Behavior planning， trajectory generation。
- Search： 找到可以到達目標的所有路徑。一般類似於A*這種演算法會在這個階段被執行並得到相關的結果。
2. Prediction： 因為周邊環境是動態的，所以把只考慮K時刻狀態下做出來的路徑規劃，用在K+1s， K+2s的未來中，是比較危險的。1，2秒之後，周邊車輛的位置肯定不是之前那個位置上了。所以車輛需要對周邊的車輛進行預測，就跟人一樣。預測的方法簡單區分的話，有兩種（或者更多，這裡只列舉兩個）
- 基於模型的預測： 根據kinetic 模型對周邊的車輛進行預測。比如Future Position = current position + velocity * prediction_time。或者利用貝葉斯濾波器，根據車輛的行為推測未來的位置。- 基於數據的預測： 藉助雲平台，查看以前所有的車輛行駛的路徑並對此建立模型。然後根據現在的狀況做出預測。（這個方法，我覺得不太行）
3. Behavior planning： 行為規劃。這個模塊的功能，相當於人類的駕駛行為，比如看情況想變道，看情況想超車，看情況想跟車，看情況想撞車（電影裡面）等等。也就是說，這個模塊是一個最上位控制器。
車輛的行為可以是- 停止- 待命- 跟前車- 超前車- 減速停車可以看出，每一個行為的背後都有關於速度，加速度，距離的一套邏輯。比如跟前車的行為，他的子命令就是跟前車保持相同的速度，保持相同的車間距。每個行為之間是可以轉換的。每個行為之間轉換的條件就是需要人為設定的。
行為規劃可以分為基於規則的，和基於獎勵的（其他的應該也有，這裡不過多敘述）。
基於規則的就是，很多個IF，THEN來判斷行為狀態是否發生改變。
基於獎勵的就是，人為設定各個行為的獎勵（或懲罰），然後根據這個獎勵（懲罰）機制，來判斷，狀態是否改發生變化。
比如下面的代碼。
代碼中的cost = 1 - exp(-(std::abs(delta_d) / distance_to_goal)) 人為設定的得到cost的機制。
double goal_distance_cost(int goal_lane, int intended_lane, int final_lane,
double distance_to_goal) {
// The cost increases with both the distance of intended lane from the goal
// and the distance of the final lane from the goal. The cost of being out
// of the goal lane also becomes larger as the vehicle approaches the goal.
int delta_d = 2.0 * goal_lane - intended_lane - final_lane;
double cost = 1 - exp(-(std::abs(delta_d) / distance_to_goal));
return cost;
}
下面圖片是基於規則的行為規劃。這是設置了ROI，然後預測後的周圍車輛在這個ROI裡面，那麼就不進行變道超車行為。
下面圖片就是ROI設定的比較小，所以就算後面的車靠的比較進，也會激進的變道超車。如果ROI設定的比較大，車輛的變道就會變得很保守。
4. Trajectory generation：生成可行駛的路徑。 「生成可行駛的路徑」這是一個比較重要的點。因為有些路徑生成之後，車輛的控制器是執行不了的，這種路徑一定要排除。為了使車輛的執行器可以執行，我們要設定車輛的路徑要足夠光滑。一般會用5次多項式來擬合車輛的路徑。因為5次多項式對於車輛來說就可以定義光滑的曲線。定義光滑的參數就是，盡量減少速度變化，同理加速度變化，同理dot（加速度）的變化。
得到光滑的曲線的關鍵在於5階多項式的係數，也就是a0-a5。一般通過初始狀態的車輛約束和路徑規劃終點的約束來求得這些係數。如何推導比較複雜，不在這裡展開。簡單來說，就和知道兩個點，求經過這兩個點的方程是一個意思。只不過這個階數高了一些。
最後會生成下面圖片中的這種waypoints。 每個waypoints擁有位置約束，速度約束，加速度約束和其他約束。車輛的執行器每個sample time就會根據這些waypoints 進行控制。
3. 總結
今天總結了一下path planning 相關的內容。總體感覺，path planning 是一個開放的模塊。很多東西取決於設定參數的人的想法。涉及的東西也很多，需要考慮的也很多。比如交通規則啊，周邊環境，乘客感受，車輛動力學什麼的都要被考慮進去，是個相當複雜的模塊。這也意味著，這個模塊的功能是核心，應該是每個自動駕駛公司的核心技術機密。
20190610
推薦閱讀：
