机器人局部避障的动态窗口法(dynamic window approach) DWA

rosparam命令可对ROS参数服务器上的参数进行操作。通过rosparam -h命令，可以看到有下面的一些方法：

Commands:
rosparam set set 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]]

机器人局部避障的动态窗口法(dynamic window approach)
https://blog.csdn.net/heyijia0327/article/details/44983551
动态窗口法主要是在速度(v,w)空间中采样多组速度，并模拟机器人在这些速度下一定时间内的轨迹。
在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。
动态窗口的含义是依据移动机器人的加减速性能限定速度采样空间在一个可执行的范围内。

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: false //设置为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: false //是否发布规划器在规划路径时的代价网格.如果设置为true,那么就会在~/cost_cloud话题上发布sensor_msgs/PointCloud2类型消息.

Oscillation Prevention Parameters

oscillation_reset_dist: 0.05 # default 0.05 //机器人运动多远距离才会重置振荡标记.

Global Plan Parameters

prune_plan: false 机器人前进是是否清除身后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值：当目标点与当前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个方向的速度样本一一组合（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重新计算出的一个一个的临时速度。
综上，生成了一个一个的不同方向和大小的小段轨迹，然后挨个去评判，轨迹的起始点为当前的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<geometry_msgs::PoseStamped>& 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:
   调用setTargetCells（）
   它的目的跟上面类似，也是定义地图中的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几乎一样，但它在构造时，定义了一个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之前的那段， 目的是为了解决有时机器人会往回走。