dwa的区别 teb_dwa杨算法解析1

准备过程：

入口 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，即可以得出距离样本 =速度