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
参考：白巧克力亦唯心的博客。