ros局部路徑規划-DWA學習

ROS的路徑規划器分為全局路徑和局部路徑規划，其中局部路徑規划器使用的最廣的為dwa，個人理解為：

首先全局路徑規划會生成一條大致的全局路徑，局部路徑規划器會把全局路徑給分段，然后根據分段的全局路徑的坐標，進行局部重新規划，例如：

全局規划后有一組目標點數組【1,2,3,4,5，......】，局部是根據當前速度和時間、約束條件，隨機模擬出50條軌跡，選出最佳一條軌跡到達1號目標點，隨后重復2,3...，

一直到達到目的地為止。具體的步驟如下：

一、路徑存儲

1、全局路徑存儲

在ros move base控制循環中會在規划出新的路徑時，將新的全局路徑利用setPlan傳給DWAPlannerROS，直接保存為global_plan_。

此時的路徑時相對於全局地圖的全局坐標系的（通常為"map")。

 

2、局部路徑存儲

在計算速度前，會先將全局路徑映射到局部地圖坐標系下（通常為“odom”），在getLocalPlan中，先將較長的全局路徑映射並截斷到局部地圖內（即坐標系轉換為局部地圖，

且范圍完全在局部地圖內，超出地圖的則拋棄，局部地圖為costmap），局部地圖使用的是rolling_window。然后，裁減全局路徑和局部路徑（與機器人當前位置距離超過1m的舊的路徑會被裁減掉）。

有了局部的目標點，就可以做規划了。

 

二、機器人運動模型

運動模型推到是為了進行軌跡推演，具體可參考：http://blog.csdn.net/heyijia0327/article/details/44983551

 

三、速度采樣

機器人的軌跡運動模型有了，根據速度就可以推算出軌跡。因此只需采樣很多速度，推算軌跡，然后評價這些軌跡好不好就行了。但是，速度（Vx,Vw）理論上有無窮多組，假定是在兩輪差速輪上，

因此根據機器人自身的條件和環境的限制，速度采樣范圍需要做一定的限制：

1、移動機器人受自身最大速度最小速度的限制 
 
2、 移動機器人受電機性能的影響:由於電機力矩有限,存在最大的加減速限制,因此移動機器人軌跡前向模擬的周期sim_period內,存在一個動態窗口,在該窗口內的速度是機器人能夠實際達到的速度: 
 
3、 基於移動機器人安全的考慮:為了能夠在碰到障礙物前停下來, 因此在最大減速度條件下, 速度有一個范圍: 

 

 

 

在獲取空間邊界時，根據use_dwa參數，采用兩套策略。

如果use_dwa==false，首先用當前位置與目標位置的距離處理仿真時間sim_time（模擬仿真時間內勻減速到0，剛好到達目標點的情景）得到max_vel_x和max_vel_y邊界，然后基於acc_lim * sim_time得到一種邊界（上邊界），還有設置的速度極限參數（max_vel_xxx，min_vel_xxx）作為一種邊界，然后選取三種邊界中空間較小的邊界。這種策略，能夠獲得較大的采樣空間（因為用了sim_time）。

如果use_dwa==true，則直接用acc_lim * sim_period得到邊界，然后還有設置的速度極限參數作為邊界，然后選取兩種邊界中空間較小的邊界。

得到速度空間邊界后，根據x，y，theta三個采樣個數進行插補，進而組合出整個速度采樣空間。

 

每個維度速度需要采樣的養本數存放在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_的容器里。至此，速度采樣就完成了。 

 

 

 

 

四、評價函數

根據采樣的速度模擬推演出相對應的軌跡，然后對每一條軌跡進行評價，找出最優的一條軌跡進行局部導航。如何找出最優的軌跡？這里DWA使用的是一個打分系統，

原始的論文只有3個評分項：

最后對每一個項做歸一化處理，目的是進行平滑。

評價函數的意義就是：找出一條最優路徑，使得機器人在局部導航過程中，避開障礙物，以最快的速度到達目標點。

通過上述幾種評分機制，選取最優的一組速度樣本，傳遞給move_base，並發布相應的local plan。move_base如果收到了可用的速度則發布給底盤，否則發布0速度，且如果尋找最優速度的時間超過了限制就會執行障礙物清理模式，state_會變為CLEARING。

 

五、停止判斷

對於DWAPlannerROS的停止處理邏輯，由LatchedStopRotateController類完成，主要包括了停止判斷、加速停止、旋轉至目標朝向三個大的部分。

在到達目標位置的前提下，還要判斷機器人朝向是否滿足目標朝向yaw_goal_tolerance的需求，如果也滿足，則判斷當前速度是否滿足停止條件，即x和y的速度小於trans_stopped_velocity，theta速度小於rot_stopped_velocity。

只有到達位置、達到朝向、速度滿足停止三個條件都滿足的情況下，才算機器人到達了目標位姿，isGoalReached函數才會返回true。

 

參考：

https://www.cnblogs.com/sakabatou/p/8297479.html

http://blog.csdn.net/heyijia0327/article/details/44983551