准备过程：

入口 bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)

1、 planner_util_.getLocalPlan：将全局路径截取到局部的costmap中。

2、updatePlanAndLocalCosts：为每一个地图代价函数设定目标点

·path_costs_——设定局部global path的终点为目标，初始化scale值=(resolution * pdist_scale_ * 0.5)。

·goal_costs_——设定局部global path的终点为目标，初始化scale值=(resolution * gdist_scale_* 0.5)。。

·goal_front_costs_——设定forward_point_distance处的点为目标，初始化scale值=(resolution * gdist_scale_* 0.5)。。。

·alignment_costs_——设定局部global path的终点为目标，同时会设定scale值：当目标点与当前pose的直线距离>forward_point_distance_ * forward_point_distance_ * cheat_factor_时，设置为resolution * pdist_scale_ * 0.5，否则为0。 其中cheat_factor_默认为1.0

·oscillation_costs_——  scale始终为1、

对这个要注意的是，在靠近最终目标的时候，这个scale就会变成0，意味着此时不再考虑这个cost。

1、prunePlan： 参数为true表示当机器人移动1米后，将1米之前的global路径点一个一个清除。(包括全局的global path和局部的global path)

2、forward_point_distance：默认0.325；将当前点与局部的global path终点连线，并延长forward_point_distance的距离作为附加的评分点。

3、publish_traj_pc： 打开后可以看到局部的轨迹发布。

4、   vx_samples: 6       # 3

vy_samples: 1       # diff drive robot, there is only one sample

vtheta_samples: 20  # 20

表示各个速度的允许采样样本数。

5、 max_trans_vel：平移速度； 可以视为x y速度的平方和开根号。

获取轨迹的过程：

simple_trajectory_generator.cpp

1、根据限制条件比如允许的最大最小速度、加速度等，以及当前的速度，计算所允许的采样速度限制：

例如：max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);

max_vel[0] 表示x方向的允许最大速度，vel[0]是当前x速度，acc_lim[0]是x轴的加速度限制。 sim_period_=1/controller_frequency

2、在速度的最大最小值之间，根据允许的采样个数(vx_samples)进行均分取点。将3个方向的速度样本一一组合(6*1*20个)，生成一系列的完整速度样本。

3、每次取一个速度样本，根据参数  sim_time，即可以得出距离样本 =速度样本*  sim_time，这里分线速度和角速度。然后再对每一个距离样本点进行进一步的分隔，分隔单位为sim_granularity和angular_sim_granularity，默认分别是0.025m和0.1rad，分隔的步数其实就= 距离样本/sim_granularity、弧度样本/angular_sim_granularity，然后取最大值(这里面得到步数的目的是为了将轨迹分成一个一个的point，轨迹长度不受此影响)。

生成的轨迹：

traj.xv_     = sample_target_vel[0];

traj.yv_     = sample_target_vel[1];

traj.thetav_ = sample_target_vel[2];//即速度样本的速度，这里没有再对速度样本进行分割，就全部都直接取样本了。

traj.time_delta_ = sim_time_ / 步数;

std::vector traj.x_pts_; ///< @brief The x points in the trajectory

std::vector traj.y_pts_; ///< @brief The y points in the trajectory

std::vector traj.th_pts_;  //

这一系列的point的计算方式： pos=pos+样本速度*dt。dt即分割后的时间，但注意样本速度没有分割。

这里有个参数：continued_acceleration_  ，如果为true，上面轨迹中的速度就不能全部是样本速度，而是将样本速度分割后，根据加速度和dt重新计算出的一个一个的临时速度。

综上，生成了一个一个的不同方向和大小的小段轨迹，然后挨个去评判，轨迹的起始点为当前的pose。

所以，轨迹的长度取决于当前的速度、参数sim_time。

评价过程:

1、bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector* all_explored)

初始化好评价函数，critics_->prepare() ：

·对网格地图评价函数：把地图所有点的target_dist设置为map.size+1，调用setLocalGoal设置target_dist。

·对障碍物评价函数：空。

拿到一小段轨迹，调用double SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost)

·调用每一个评价函数的scoreTrajectory去评价，评价结果即为cost。

·cost小于0则直接返回cost，

·否则，将cost*每一个评价函数的scale进行调整，然后将所有的评价进行加和。

·与best_traj_cost相比，取较小者为best_traj_cost(不能为负)。

·比较例外的是震荡评价函数的scoreTrajectory，它是用来避免发生轨迹震荡的。

·

1.什么叫震荡——例如 ：当连续出现2个x速度<0.1, z速度为正，然后又为负的最佳轨迹，然后再出现一个z速度为正的轨迹去评价时，则这个轨迹就会被认为是震荡。

得到最佳轨迹，放入traj中，返回值表示是否获取到最佳轨迹(遍历完所有轨迹后，得到的best_traj_cost<0则表示拿不到最佳轨迹)。

2、理解评价函数的评价机制：

网络地图准备过程：

2.1  、

goal_cost：

调用 void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap, const std::vector<:posestamped>& global_plan)

将原先的global plan进行扩大(如果plan的点间隔大于4倍的网格，就在中间插入新点)，沿着plan寻找到第一个达到costmap边界的点(不关心障碍物)(考虑锐角折线会被截成两段孤立的线段怎么处理？)；把这个点加入到queue path_dist_queue中，当前这个点的target_dist=0，target_dist代表这个点离path的距离，显然当前点就在path上。

·从path_dist_queue取出front点；将这个点从path_dist_queue中pop出去

·然后取得当前点的上下左右邻居点，如果它们是障碍物或者空洞，将它们的target_dist设置为极大值(map.size)；如果不是，将它们的target_dist=当前点的+1或者不变(取较小的那个)，并将这些点也加入到path_dist_queue中。

·上述过程不断重复，结果就是在不断的扩充，直到遇到障碍物或空洞则停止那个方向的点的扩充。 最终目的应该是得到了目标路径点周边所有点的target_dist。

path_cost:

调用setTargetCells()

它的目的跟上面类似，也是定义地图中的target_dist。区别在于，goal_cost中的target_dist都是以局部的目标终点为基准进行+1的延伸；而这里则是将path中的所有点从头到尾加入到queue path_dist_queue中，然后分别扩展，所以它的target_dist代表着地图中其它点到path的距离代价。

这样，goal_cost和path_cost的代价就区分出来了。

2.2 double MapGridCostFunction::scoreTrajectory(Trajectory &traj)

对轨迹中的每一个点，取其target_dist当做cost值return出去。(注意下不同类型时，返回的cost值计算方式不一样，以轨迹中哪一点的cost为准呢？)

如果点越界了，则return -4.0；如果target_dist==map.size,则return -3.0；如果target_dist ==map.size+1，则return -2.0.

另注：alignment_costs_：

它跟path_cost几乎一样，但它在构造时，定义了一个xoffset= forward_point_distance_，这样在计算代价时，它计算的刚好是对轨迹path中的每一个点取其x方向的偏移xoffset*cos(pth)后的点来计算target_dist。

goal_front_costs_与上面类似，所以就算是轨迹在原地旋转，得到的cost也是不同的。

障碍物地图准备过程：

2.3  首先理解footprint_spec的概念，footprint_spec是一系列多边形的顶点，它的坐标系是机器人中心坐标系可以认为为base_link. 所以，footprint_spec中的点在地图中的坐标点应该为：

new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);

new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);

其中， x y th为机器人的中心坐标系位置，也就是一般意义的机器人位置。

获取footprint cost—— 其实就是footprint_spec中两两端点连成线段，计算所有线段中的点的最大cost。同样，cost值要么为costmap中记录的cost值，要么如果是障碍物、空洞、内切(圆形才会判断内切) 则返回-1作为cost值。

2.4 计算代价double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) ：

将traj中的每一个点都扩展出一个footprint_spec，计算出其footprint cost

·只要有一个点返回的footprint cost<0,则返回这个值。

·如果sum_scores为true，则将所有点的footprint cost加和返回；否则，返回最后一个点的footprint cost。

参数调节心得：

一、默认path_distance_bias: 32.0   goal_distance_bias: 24.0。

在某些情况下出现的问题：

机器人开始时，得到的一系列最优速度为：

0.267  0  0.667

0.240  0  0.632

0.160  0  0.456

0.000  0  0.105

0.000  0  0.667

……………………

0.000  0  0.140

0.000  0  -0.526

可以看出后面的速度已经出现问题了，机器人的表现就是在一直在向左旋转，从评价结果可以知道如果选择其它轨迹，有下面的问题：

1、选择一个x方向有速度，z速度仍为0.140的轨迹进行评价，发现其goal和goal-front都会变小，但path和path_align增大，并且后者幅度大于前者，导致原地旋转优于向左前运动。

2、选择一个x方向速度为0，z速度为-0.140的轨迹进行评价，发现其path和path_align变小，但goal_front变大(goal不变)，并且后者幅度大于前者，导致原地逆时针转优于原地顺时针转。

作为使用者，我们要么希望它更贴近path走，要么希望它直接往goal走，但不希望它原地转，这表明我们需要把path和goal的bias差距变大一点，这样就不会出现上述的震荡情况(差距不明显，导致哪个都无法选择)。

另外，还可以认识到，path_align是通过机器人的轨迹方向来影响到轨迹评价的，而且forward_point_distance_是会加重这个影响。

二、将goal_distance_bias调小成2.0

此时机器人一般会贴着path走，但在上图中三角形顶点O附近，它不会严格走到顶点，而是以一道弧线从附近直接走到下一个边，即AB而非AOB。原因在于，在计算path_cost时，取得是最后一个轨迹点的cost，而这个轨迹点在靠近B点时的path代价也很小，再加上它的goal代价较之O点要小，所以结果就会选择趋近于B点的轨迹为最优。

注意到path计算cost时是以轨迹最后一点的cost为算，而非将所有点的cost加起来，如果非要改成加和的形式的话，会导致原地旋转的path cost相对来说就会特别小。

但这个时候有下面的问题：

本应该A到B，但却从A的位置，顺时针旋转然后原路返回了，这个我认为应该是goal cost太少，从A出发的轨迹中AB的path cost比A原地旋转还要大，并且大很多，而goal cost虽然B较小，但不够小。

三、清理掉已经走过的path：

保持上面的配置，考虑到有时候会原路返回，所以设想如果能清除掉走过的路径就好了，参数中有个prune_plan,它的原理是从global_plan的起始点开始，检测与当前pose的距离>1则清除掉这些点直到遇到第一个距离小于1的点停止。

如果设想把这个检测条件改小比如0.01，则问题在于如果机器人轨迹规划到AB中间，则所有的路径点都会小于0.01，结果就会将所有路径清除，导致movebase重新规划路径，特别是使用pose estimate功能时问题更显然。 当然，这样确实能解决原路返回的问题，只是检测条件要稍大一点比如0.1，此时倒也不会重新规划路径。

所以，最优办法是能够确定哪些路径点是走过的 。

现在的做法是：理论上如果能选取到当前global path上距离当前pose最近的点(就是作垂直线取交点)，然后往下找路径，这样就不会找到走过的路径了。问题是如何取到这个最近点，一般做法是遍历所有点，但太浪费时间，而如果仅仅遍历到costmap范围的path点呢，则解决不了那种弯弯绕的路径(可能会被costmap切割成几段)。正确的做法是仍然进行遍历，这样最安全，但只是在第一次时遍历一次(或者也不需要)，然后记录下这次取到的最近点，下次运行dwa时使用这个记录点往两个方向去查找，最多查找costmap半径的距离点即可，这个基于我们认为两次dwa时的最近点不会相距很远，应该是非常近。所以，如果中途进行了人为的pose定位，会影响到这个。

调试心得：

1、有时在转弯时会撞墙，原因在于靠墙太近，检测不到障碍物，估计DWA规划让它撞的墙，因为DWA只考虑局部地图，不关心静态地图。

思路1： 考虑将全局静态地图加入到DWA的costmap层中。—— 结果确实可以避免撞墙，但如果这样做的话，amcl定位有时会有误差，这样就会将错误的静态地图障碍引入到costmap中，导致dwa规划有问题。

所以，我认为局部规划不应该关心静态地图，因为总是会有误差，仅应当关心实际存在的障碍物，不应该关心任何先验的因素。

思路2：增大膨胀层，让机器人尽量避开障碍物；增加红外。—— 可行。

2、在狭窄过道中，由于定位的漂移，会导致机器人认为自己进入障碍物。

思路1： 我决定修改movebase，当机器人陷入困境时，让自由行走发挥作用，然后不断的去get当前pose，可行的话就继续进行局部规划，每次自由行走的时间留短一些，但可以执行多次。

3、仿照teb的程序，在取局部的global planner的时候改成不取当前pose之前的那段， 目的是为了解决有时机器人会往回走。