DWA算法分析
DWA Local Planner這部分是屬于Local planner，在Navigation中有兩個包：
dwa_local_planner
和
base_local_planner
檢視了
dwa_local_planner
，發現其實際是調用的
base_local_planner
中的函數，而
base_local_planner
中包含了兩種planner ：
DWA
或者
Trajectory Rollout approach
。是以結論就是，隻需要搞清楚
base_local_planner
的執行就OK。
base_local_planner
package 實際是繼承于
BaseLocalPlanner
DWA算法實作分為四個部分
1.運動模型的建立
2.速度采樣
3.評價函數
具體步驟如下
1.首先建立機器人的運動模型，退到出運動模型的航迹推演公式。
2.運動模型建立完畢後，接下來要進行速度采樣。
采樣速度的重點是要将機器人的速度限制在一定的範圍内（v，w）
dist（vw）為機器人裡障礙物最近的距離（并不是一開始就可以得到的，而是根據機器人遇到障礙物時這組速度可不可以停下來，如果可以停下來，那麼這組資料就是可以接受的。如果停不下來，那就接受不了）
3.評價函數：
heading（v ，w）達到模拟軌迹末端時的朝向和目标之間的角度差距。dist（v，w）表示目前軌迹上和障礙物的差距。velocity（v w）用來評價目前軌迹速度的大小。
三個部分在歸一化後進行相加。
demo：
- EGIN DWA(robotPose,robotGoal,robotModel)
- laserscan = readScanner()
- allowable_v = generateWindow(robotV, robotModel)
- allowable_w = generateWindow(robotW, robotModel)
- for each v in allowable_v
- for each w in allowable_w
- dist = find_dist(v,w,laserscan,robotModel)
- breakDist = calculateBreakingDistance(v)
- if (dist > breakDist) //can stop in time
- heading = hDiff(robotPose,goalPose, v,w)
- //clearance與原論文稍不一樣
- clearance = (dist-breakDist)/(dmax - breakDist)
- cost = costFunction(heading,clearance, abs(desired_v - v))
- if (cost > optimal)
- best_v = v
- best_w = w
- optimal = cost
- set robot trajectory to best_v, best_w
- END
參考：白巧克力亦唯心的部落格。