區域性路徑規劃
區域性路徑規劃簡介
機器人在獲得目的地資訊後，首先經過全域性路徑規劃規劃出一條大致可行的路線，然後呼叫區域性路徑規劃器根據這條路線及costmap的資訊規劃出機器人在區域性時做出具體行動策略，ROS中主要是使用了DWA演算法。在ROS中每當move_base處於規劃狀態就呼叫DWA演算法計算出一條最佳的速度指令，傳送給機器人運動底盤執行。

DWA演算法
DWA演算法全稱為dynamic window approach，其原理主要是在速度空間（v,w）中取樣多組速度，並模擬這些速度在一定時間內的運動軌跡，再通過一個評價函式對這些軌跡打分，最優的速度被選擇出來傳送給下位機。
1.確定機器人模型
在動態視窗演算法中，要模擬機器人的軌跡，需要知道機器人的運動模型。假設兩輪移動機器人的軌跡是一段一段的圓弧或者直線（旋轉速度為0時），一對（vt，wt）就代表一個圓弧軌跡,具體推導如下:
假設不是全向運動機器人，它做圓弧運動的半徑為：

當旋轉速度w不等於0時，機器人座標為：


2.速度取樣
機器人的軌跡運動模型有了，根據速度就可以推算出軌跡。因此只需取樣很多速度，推算軌跡，然後評價這些軌跡好不好就行了。
（一）移動機器人受自身最大速度最小速度的限制

（二） 移動機器人受電機效能的影響:由於電機力矩有限,存在最大的加減速限制,因此移動機器人軌跡前向模擬的週期sim_period內,存在一個動態視窗,在該視窗內的速度是機器人能夠實際達到的速度:

（三） 基於移動機器人安全的考慮:為了能夠在碰到障礙物前停下來, 因此在最大減速度條件下, 速度有一個範圍:

3.評價函式
當取樣完速度後，就對每個速度所形成的每條軌跡進行評價，採用的評價函式如下：

區域性路徑規劃程式架構
1.程式模組流程

（1）初始化：為DWA演算法做準備，載入引數和例項化物件等
（2）取樣速度樣本：計算出需要評價的速度樣本
（3）樣本評分：對計算出的速度樣本進行逐一評分，記錄下評價最高的樣本
（4）釋出plan：釋出得到的區域性路徑規劃策略
2.DWA演算法程式分析
（1）初始化：
在move_base節點中，通過類載入模組載入了BaseLocalPlanner（區域性路徑規劃）的子類DWAPlannerROS的例項tc_，並呼叫其初始化函式，獲取了一些初始狀態資訊比如機器人當前位置等，並建立了真正實現DWA演算法的DWAPlanner類的例項dp_，最後設定了動態引數配置服務。dp_的建構函式做了一系列引數獲取的操作，最重要的是將幾種cost計算方法的例項加入一個名為critics的vector容器裡。
（2）取樣速度樣本：
當move_base呼叫tc_的computeVelocityCommands方法後，tc_會呼叫dwaComputeVelocityCommands方法，並在其中呼叫dp_的findBestPath方法。findBestPath方法裡呼叫SimpleTrajectoryGenerator類的例項generator_的initialise函式，這個函式就是主要負責速度取樣的。
每個維度速度需要取樣的養本數存放在vsamples_這個結構體內，vsamples_[0]是x方向樣本數，vsamples_[1]是y方向樣本數，vsamples_[2]是z方向樣本數。首先計算各個方向的最大速度和最小速度，DWA演算法只在第一步進行取樣，所以最大速度為:
Max_vel=max(max_vel,vel+acc_lim*sim_period)
最小速度為:
Min_vel=min(min_vel,vel-acc_lim*sim_period)
其中max_vel，min_vel為人為設定的最大和最小速度，vel是當前速度，acc_lim是人為設定的最大加速度，sim_period是第一步的模擬時間，由人為設定的區域性路徑規劃頻率決定，預設為0.05。
當計算出各個維度的最大最小速度後，就建立三個VelocityIterator類的物件，並傳入最大最小速度和樣本數目，此物件的建構函式會生成同樣數目的速度樣本並放入samples_這個容器內。具體做法是先計算步長step_size：
step_size=(max-min)/(nums_samples-1)
max為最大速度，min為最小速度，nums_samples為樣本數目。從最小速度每次多累加一次step_size即為一個速度樣本，直到達到最大速度。將每個維度的速度樣本取得後，再全部迴圈每個樣本組裡選擇一個組合放入結構體vel_sample，最後將這些vel_sample放入sample_params_的容器裡。至此，速度取樣就完成了。
（3）樣本評分
速度取樣完成後，逐一迴圈對樣本空間內的樣本進行評分。對每一組速度呼叫scoreTrajectory函式計算其評分，而scoreTrajectory函式則對這一組速度呼叫所有critics容器裡的costfunction計算每個cost從而累加算出總的cost。在計算過程中，一旦累加的cost大於當前最小的cost則拋棄這組速度。
之前說到的幾種cost成本函式為下列所示：
ObstacleCostFunction
這個成本函式基於感知障礙物來評估軌跡。它或者由於軌跡通過障礙物而返回負值，或者0。
MapGridCostFunction
這個成本函式類基於軌跡離全域性路徑或者接近目標點有多近來評估軌跡。這個嘗試利用距離預計算地圖有相同距離的路徑或者目標點的所有的規劃，來優惠計算速度。
在 dwa_local_planner中，代價函式因為不同的目的，被多次例項化。保持軌跡接近於路徑，使機器人朝區域性目標前進，並且使機器人的前段點指向區域性目標。代價函式是一個啟發，可以帶來壞的結果或者不合適的引數的失敗。
OscillationCostFunction
震盪發生在X,Y,theta維度上，正/負值被連續的選擇。為了阻止震盪，當機器人在任何方向移動時，與下一個迴圈相反的方向被標記為無效，直到機器人已經從所設定標記的位置移動而並且超過一定的距離。這個成本函式類幫助減少某些震盪，雖然這可以有效的阻止這些震盪，如果使用不合適的引數，但是有可能阻止良好的解。
PreferForwardCostFunction
考慮到好的鐳射掃描範圍只在機器人的前面，這個成本函式類被設計在像PR2一樣的機器人上。成本函式更喜歡正面向前運動，懲罰背面運用及掃射動作。在其他機器人上或者其他領域，這可能是非常不可取的行為。
（4）釋出plan
通過上述幾種評分機制，選取最優的一組速度樣本，傳遞給move_base，併發布相應的local plan。move_base如果收到了可用的速度則釋出給底盤，否則釋出0速度，且如果尋找最優速度的時間超過了限制就會執行障礙物清理模式，state_會變為CLEARING。