局部路徑規劃
局部路徑規劃簡介
機器人在獲得目的地信息後，首先經過全局路徑規劃規劃出一條大致可行的路線，然後調用局部路徑規劃器根據這條路線及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。
部分參考自 http://blog.csdn.net/heyijia0327/article/details/44983551