机器⼈局部避障的动态窗⼝法（dynamicwindowapproach）DWA
rosparam命令可对ROS参数服务器上的参数进⾏操作。通过rosparam -h命令，可以看到有下⾯的⼀些⽅法：
Commands:
rosparam t t parameter 设置参数
rosparam get get parameter 获得参数值
rosparam load load parameters from file 从⽂件中加载参数到参数服务器
rosparam dump dump parameters to file 将参数服务器中的参数写⼊到⽂件
rosparam delete delete parameter 删除参数
rosparam list list parameter names 列出参数服务器中的参数
机器⼈局部避障的动态窗⼝算法DWA (dynamic window approach)
机器⼈运动学模型参数
最⾼速度m/s]
最⾼旋转速度[rad/s]
加速度[m/ss]
旋转加速度[rad/ss]
速度分辨率[m/s]
转速分辨率[rad/s]]
1.在机器⼈控制空间离散采样(dx, dy, dtheta)
2.对每⼀个采样的速度进⾏前向模拟，看看在当前状态下，使⽤该采样速度移动⼀⼩段时间后会发⽣什么.
3.评价前向模拟得到的每个轨迹，是否接近障碍物，是否接近⽬标，是否接近全局路径以及速度等等.舍弃⾮法路径
4.选择得分最⾼的路径，发送对应的速度给底座
DWA与Trajectory Rollout的区别主要是在机器⼈的控制空间采样差异.Trajectory Rollout采样点来源于整个前向模拟阶段所有可⽤速度集合,⽽DWA采样点仅仅来源于⼀个模拟步骤中的可⽤速度集合.这意味着相⽐之下DWA是⼀种更加有效算法，因为其使⽤了更⼩采样空间；然⽽对于低加速度的机器⼈来说可能Trajectory Rollout更好, 因为DWA不能对常加速度做前向模拟。
DWAPlannerROS:
Robot Configuration Parameters - stdr robot
acc_lim_x: 0.3 # maximum is theoretically 2.0//x⽅向的加速度绝对值
acc_lim_y: 0.0 # diff drive robot//acc_lim_y:y⽅向的加速度绝对值，该值只有全向移动的机器⼈才需配置.
acc_lim_th: 0.3 // acc_lim_th：旋转加速度的绝对值.
max_trans_vel: 0.3 //平移速度最⼤值绝对值
min_trans_vel: 0.1 //平移速度最⼩值的绝对值
max_vel_x: 0.3 //x⽅向最⼤速度的绝对值
min_vel_x: -0.1 //x⽅向最⼩值绝对值，如果为负值表⽰可以后退.
max_vel_y: 0.0 //y⽅向最⼤速度的绝对值.
min_vel_y: 0.0 //y⽅向最⼩速度的绝对值.
max_rot_vel: 0.5 // 最⼤旋转速度的绝对值.
min_rot_vel: 0.1 // 最⼩旋转速度的绝对值.
Goal Tolerance Parameters
yaw_goal_tolerance: 0.1 # 0.1 rad = 5.7 degree //到达⽬标点时偏⾏⾓允许的误差，单位弧度.
xy_goal_tolerance: 0.12 // 到达⽬标点时，在xy平⾯内与⽬标点的距离误差.
latch_xy_goal_tolerance: fal //设置为true，如果到达容错距离内，机器⼈就会原地旋转，即使转动是会跑出容错距离外.
Forward Simulation Parameters
sim_time: 2.0 # 1.7 //向前仿真轨迹的时间.
sim_granularity: 0.025 //步长，轨迹上采样点之间的距离，轨迹上点的密集程度.
vx_samples: 6 # default 3 //x⽅向速度空间的采样点数.
vy_samples: 1 # diff drive robot, there is only one sample//y⽅向速度空间采样点数.
vth_samples: 20 # 20 //旋转⽅向的速度空间采样点数.
经典好听
controller_frequency: 5.0 //发送给底盘控制移动指令的频率.
Trajectory Scoring Parameters
path_distance_bias: 90.0 # 32.0 //定义控制器与给定路径接近程度的权重.
goal_distance_bias: 24.0 # 24.0 //定义控制器与局部⽬标点的接近程度的权重.
occdist_scale: 0.3 # 0.01 //定义控制器躲避障碍物的程度.
forward_point_distance: 0.325 # 0.325 //默认0.325；将当前点与局部的global path终点连线，并延长forward_point_distance的距离作为附加的评分点。
stop_time_buffer: 0.2 # 0.2 //为防⽌碰撞，机器⼈必须提前停⽌的时间长度.
scaling_speed: 0.20 # 0.25 //启动机器⼈底盘的速度.
max_scaling_factor: 0.2 # 0.2 //最⼤缩放参数.
publish_cost_grid: fal //是否发布规划器在规划路径时的代价⽹格.如果设置为true，那么就会在~/cost_cloud话题上发布
nsor_msgs/PointCloud2类型消息.
Oscillation Prevention Parameters
oscillation_ret_dist: 0.05 # default 0.05 //机器⼈运动多远距离才会重置振荡标记.
电音英文Global Plan Parameters
prune_plan: fal 机器⼈前进是是否清除⾝后1m外的轨迹.
准备过程：
⼊⼝ 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值：当⽬标点与当前po的直线距离
>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个⽅向的速度样本⼀⼀组合（6120个），⽣成⼀系列的完整速度样本。
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重新计算出的⼀个⼀个的临时速度。
综上，⽣成了⼀个⼀个的不同⽅向和⼤⼩的⼩段轨迹，然后挨个去评判，轨迹的起始点为当前的po。
所以，轨迹的长度取决于当前的速度、参数sim_time。
评价过程:
1、bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector all_explored)
初始化好评价函数，critics_->prepare() ：
· 对⽹格地图评价函数：把地图所有点的target_dist设置为map.size+1，调⽤tLocalGoal设置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::tLocalGoal(const costmap_2d::Costmap2D& costmap, const定期理财
std::vector<geometry_msgs::PoStamped>& global_plan)
将原先的global plan进⾏扩⼤（如果plan的点间隔⼤于4倍的⽹格，就在中间插⼊新点），沿着plan寻找到第⼀个达到costmap边界的点（不关⼼障碍物）（考虑锐⾓折线会被截成两段孤⽴的线段怎么处理？）；把这个点加⼊到queue<MapCell*> 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:
调⽤tTargetCells（）
它的⽬的跟上⾯类似，也是定义地图中的target_dist。区别在于，goal_cost中的target_dist都是以局部的⽬标终点为基准进⾏+1的延伸；⽽这⾥则是将path中的所有点从头到尾加⼊到queue<MapCell*> 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⼏乎⼀样，但它在构造时，定义了⼀个xofft= forward_point_distance_，这样在计算代价时，它计算的刚好是对轨迹path中的每⼀个点取其x⽅向的偏移xofft*cos(pth)后的点来计算target_dist。
goal_front_costs_与上⾯类似，所以就算是轨迹在原地旋转，得到的cost也是不同的。
障碍物地图准备过程：
2.3 ⾸先理解footprint_spec的概念，footprint_spec是⼀系列多边形的顶点，它的坐标系是机器⼈中⼼坐标系可以认为为
ba_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 的起始点开始，检测与当前po的距离>1则清除掉这些点直到遇到第⼀个距离⼩于1的点停⽌。
如果设想把这个检测条件改⼩⽐如0.01，则问题在于如果机器⼈轨迹规划到AB中间，则所有的路径点都会⼩于0.01，结果就会将所有路径清除，导致moveba重新规划路径，特别是使⽤po estimate功能时问题更显然。 当然，这样确实能解决原路返回的问题，只是检测条件要稍⼤⼀点⽐如0.1，此时倒也不会重新规划路径。
所以，最优办法是能够确定哪些路径点是⾛过的 。
现在的做法是：理论上如果能选取到当前global path上距离当前po最近的点（就是作垂直线取交点），然后往下找路径，这样就不会找到⾛过的路径了。问题是如何取到这个最近点，⼀般做法是遍历所有点，但太浪费时间，⽽如果仅仅遍历到costmap范围的path点呢，则解决不了那种弯弯绕的路径（可能会被costmap切割成⼏段）。正确的做法是仍然进⾏遍历，这样最安全，但只是在第⼀次时遍历⼀次（或者也不需要），然后记录下这次取到的最近点，下次运⾏dwa时使⽤这个记录点往两个⽅向去查找，最多查找costmap半径的距离点即可，这个基于我们认为两次dwa时的最近点不会相距很远，应该是⾮常近。所以，如果中途进⾏了⼈为的po定位，会影响到这个。
