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。

dwa算法demo

 来源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html BEGIN 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

五、停止判断

对于DWAPlannerROS的停止处理逻辑，由LatchedStopRotateController类完成，主要包括了停止判断、加速停止、旋转至目标朝向三个大的部分。

在到达目标位置的前提下，还要判断机器人朝向是否满足目标朝向yaw_goal_tolerance的需求，如果也满足，则判断当前速度是否满足停止条件，即x和y的速度小于trans_stopped_velocity，theta速度小于rot_stopped_velocity。

只有到达位置、达到朝向、速度满足停止三个条件都满足的情况下，才算机器人到达了目标位姿，isGoalReached函数才会返回tru