2，行為規劃
行為規劃（Behavior Planning）是無人車規劃模塊的一層，位於全局任務規劃和底層的動作規劃層之間，駕駛行為規劃也被稱為駕駛行為決策，這一層的作用主要是依據來自上層（任務規劃層）的全局最優行駛路線信息，根據當前的交通場景和環境感知的信息的理解，來確定自身當前駕駛狀態，在交通規則的約束和駕駛經驗的指導下規划出合理的駕駛行為。
上面這張圖顯示了整個工作流程，最左邊的時鐘表示每個模塊運行一次所需要的時間。
其思想是根據最小成本函數來選擇一個最優狀態來進行轉移，當然前提是這個狀態是可以轉移的狀態。
成本函數的設計會考慮以下幾個方面並按這個重要性進行排序：
可行性 》安全性 》合法性 》舒適性 》高效性
但是在特定情境下，比如紅綠燈路口，就需要首先考慮合法性，所以在每個因素前面乘以一個權重w來改變重要性。
在高速道路上，其車輛行駛的狀態空間較小，可以使用有限狀態機來進行行為規劃，在城市道路上，其狀態空間較大，FSM可能就有些困難了。這時候就可以考慮分層有限狀態機（HFSM）。
當狀態很多的時候，有限狀態機就有可能變得非常龐大，假設有限轉態機有N中狀態，那麼其可能的狀態轉換就有 N×NN×N 種，當 NN 的數量很大的時候，狀態機的結構也會變得更加複雜，此外，有限狀態機還存在如下幾個問題：
- 可維護性差：當新增或者刪除一個狀態的時候，需要改變所有與之相關聯的狀態，所以對狀態機的大幅度的修改很容易出錯；
- 可擴展性差：當FSM包含大量狀態時，有向圖可讀性很差，不方便擴展；
- 復用性差：幾乎不可能在多個項目中使用相同的FSM
這時候就可以使用分層狀態機(Hierarchical Finite-State Machine)了，把那些同一類型的狀態機做為一個狀態機，然後再做一個大的狀態機，來維護這些子狀態機，相比於FSM，HFSM新增了一個超級狀態（Super-state）：本質上也就是將性質同一類型的一組狀態何為一個集合，超級狀態之間也有轉移邏輯。這也就意味著HFSM不需要為每一個狀態和其他所有狀態建立轉移邏輯，由於狀態被歸類，類和類之間存在轉移邏輯，那麼類和類之間的狀態轉移可以通過繼承這個轉移邏輯來實現，這裡的轉換繼承就像面向對象編程中通過多態性讓子類繼承超類一樣。
比如：對小狗來說，我們可以把小狗的狀態先定義為疲勞，開心，憤怒，然後這些狀態里再定義小狀態，比如在開心的狀態中，有撒橋，搖尾巴等小狀態，這樣我們在外部只需要關心三個狀態的跳轉（疲勞，開心，憤怒）。如果不用HFSM的話，就會有很多跳轉鏈接；如果用HFSM的話，在每個狀態的內部只需要關心自己的小狀態的跳轉就可以了，這樣就大大的降低了狀態機的複雜度和跳轉鏈接數。如下圖：
「Junior」是斯坦福大學在2007年參加DARPA城市挑戰賽時的無人車，它是第二名完成該比賽的無人車，Junior的行為規劃系統就是通過分層有限狀態機實現的。
雖然HFSM比傳統的FSM更為模塊化，但它仍然繼承了FSM大多數缺點，例如有限的可重用性。再進階，就是行為樹（Behavior Tree），行為樹這種方法將各種狀態模塊化，以便邏輯的不同部分能夠更輕鬆地重用它們。
行為樹：行為樹與FSM不同，它是一種「輪詢式機制」，即每次更新都會遍歷樹，判定邏輯是否成立，是否該繼續往下執行。如下圖
行為樹(Behavior Tree)具有如下的特性：
它只有4大類型的Node：
* Composite Node 組合節點* Decorator Node 修飾節點* Condition Node 條件節點（葉節點）* Action Node 動作節點（葉節點）任何Node被執行後，必須向其Parent Node報告執行結果：成功 / 失敗。
這簡單的成功 / 失敗彙報原則被很巧妙地用於控制整棵樹的決策方向。Condition Node和Action Node為Behavior Node，而Composite Node和Decorator Node為Decider Node，用於控制行為樹中的決策走向。
行為規劃偽代碼
實現轉換功能的一種方法是為每個可訪問的「下一個狀態」生成粗略軌跡，然後找到最佳狀態。 為了「找到最好的」，我們通常使用成本函數。 然後我們可以計算每條粗糙軌跡的成本，然後選擇成本軌跡最低的狀態。
def transition_function(predictions, current_fsm_state, current_pose, cost_functions, weights):
# only consider states which can be reached from current FSM state.
possible_successor_states = successor_states(current_fsm_state)
# keep track of the total cost of each state.
costs = []
for state in possible_successor_states:
# generate a rough idea of what trajectory we would
# follow IF we chose this state.
trajectory_for_state = generate_trajectory(state, current_pose, predictions)
# calculate the "cost" associated with that trajectory.
cost_for_state = 0
for i in range(len(cost_functions)) :
# apply each cost function to the generated trajectory
cost_function = cost_functions[i]
cost_for_cost_function = cost_function(trajectory_for_state, predictions)
# multiply the cost by the associated weight
weight = weights[i]
cost_for_state += weight * cost_for_cost_function
costs.append({state : state, cost : cost_for_state})
# Find the minimum cost state.
best_next_state = None
min_cost = 9999999
for i in range(len(possible_successor_states)):
state = possible_successor_states[i]
cost = costs[i]
if cost < min_cost:
min_cost = cost
best_next_state = state
return best_next_state
2019.3.13
今天做了behavior的c++作業，寫下總結。
作業和偽代碼差不多，就是讓你實現以下FSM，作業中，已經給出了當前狀態下的下一個可能轉移的狀態。狀態集合為("KL","PLCL","PLCR","LCL","LCR")。
不管是什麼狀態，下一個狀態一定有"KL"。
如果當前狀態為"KL"，則可以選擇"PLCL" or "PLCR"，原因之前課程提過，直接變道危險或者不利於交通。所以增加了一個預變道狀態，先估計一下變道後的車速情況，先在本車道調整。
如果當前狀態為"PLCL",則可以選擇"PLCL" or "LCL"。
如果當前狀態為"PLCR",則可以選擇"PLCR" or "LCR"。
如果當前狀態為"LCL" or "LCR"，則返回 "KL"，剛變道完，下一個狀態還是keep lane 安全些。
得到下一個狀態的集合之後，就開始遍歷，為每個狀態生成一條軌跡（作業中軌跡就只有當前時刻點和下一個時刻點這兩個組成），計算其cost，找出cost最小的索引，得到對應的軌跡。
cost函數有兩個，一個考慮距離終點距離，另一個考慮行駛速度（效率）。
如果當前車道號碼與目標車道號碼離得越遠，cost越大；如果縱向距離越近，cost越大（怕離得近了再換道，時間不夠，偏向于越早換到目標車道越好）。
如果當前車速越接近目標車速，cost越小。
參考：
udacity課程
https://blog.csdn.net/BillCYJ/article/details/79797228
https://blog.csdn.net/AdamShan/article/details/80633099
推薦閱讀：