ROS Navigation Tuning Guide

基本点：

一、速度和加速度

动力性：机器人的速度和加速度
Ros导航中，local planner 接收测距消息(odom主题)，并输出速度命令（cmd_vel主题）老控制机器人的运动
最大/最小速度和加速度是移动基站的两个基本参数，在Ros导航中，需要知道平移和旋转的速度和加速度。

二、获取最大速度（匀速状态的时候，则为最大速度）

1、订阅odom主题来获取当前的测距信息，输出里程计数据
3、平移速度（m/s）：机器人直线移动时的速度，最大值就是达到恒定速度
4、旋转速度（rad/s）：等效于角速度，最大值是机器人旋转360°，直到角速度达到恒定值

例如：我们在测试导航的时候，当设置的速度时，机器人在激光漂移的那一时刻，记下当前速度，可记为最大速度。
同时为了安全起见，我们倾向于设定最大平移速度和旋转速度低于它们的实际值。

三、获得最大加速度

1、输出带有时时间戳的里程计数据
2、记录机器人到达这个最大平移速度v需要多长时间 t
3、使用里程计数据来计算这个过程的加速度a。
4、多做几次，求平均值

相关公式：

V=V1+at  (速度=初速度+加速度*时间)
a=(V-V1)/t   单位：m/s^2

四、设置最小速度

1、最小平移速度设置为一个负值，因为这可以将机器人在陷入困境的时候后退
2、最小的旋转速度，如果参数允许，我们设置为负数，便于机器人在任意方向旋转。
3、要注意，DWA本地规划器采用的是机器人最小旋转速度的绝对值。

五、x、y方向的速度

1、x方向是平行于机器人直线运动方向的速度，与移动速度相同
2、y方向是垂直于直线运动的方向，被称为“冲击速度”
3、对于非整体机器人，y速度应该设置为0

六、机器人平移速度（矢量）

max_trans_vel：机器人的最大平移速度
min_trans_vel：机器人最小的平移速度

全局路径规划

略。。。。。

局部规划路径

一、dwa规划器

1、dwa算法（具体采用的是动态窗口的方法）

  具体执行过程

 1、将机器人的控制空间离散化（dx,dy，dtheta）
 2、2.对于每一个采样速度，从机器人的当前状态执行正向模拟，以预测如果在短时间段内采用采样速度将会发生什么
 3、评估从正向模拟产生的每个轨迹使用包含诸如：障碍物接近度、目标接近度、全局路径接近度和速度等特征的度量，
 丢弃非法轨迹（与障碍物相撞的轨迹）
 4、选择得分最高的轨迹，将相关联的速度发送给移动基站
 5.清零然后重复以上过程

dwa的原版论文：https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf
中文版的参考：https://blog.csdn.net/heyijia0327/article/details/44983551

论文介绍：
1、dwa算法的目的生成一个动作对（v,w），代表机器人最佳的圆形轨迹
2、dwa是通过在下一个时间间隔类内搜索速度空间来达到此目的
3、在这个可以接受的速度规定的圆形轨迹是最接近障碍物的时候停止
4、DWA将仅考虑动态窗口内的速度，其被定义为给定当前平移和旋转速度和加速度在下一时间间隔内可到达的速度对集合。
5、DWA最大化目标函数，其取决于（1）到目标的进展，（2）从障碍物中清除，以及（3）前进速度以产生最佳速度对。

现在，我们来看ROS Wiki的算法总结。 第一步是在动态窗口内的速度空间中的采样速度对（vx,vy,w）。第二步是消除不可接受的速度（即消除不良轨迹）。第三步是使用目标函数来评估速度对，输出轨迹得分。 第四和第五步很容易了解：采取当前最佳速度选项并重新计算。

最重要的是DWA 规划器取决于提供障碍物信息的local costmap,所以调整costmap的参数对于dwa的本地规划非常的重要

1、DWA Local Planner:向前模拟

1、DWA算法的第二步，进行速度采样，检查这些速度样本产生的轨迹，并消除不良的速度。
2、我们将sim_time视为机器人以采样速度移动的时间
3、sim_time时间越长，计算负荷越大。sim_time变长以后，local_panner产生的路径也就会越长。

4、速度采样

    1、vx_sample：x方向上平移速度样本的数量
    2、vy_sample: y方向上平移速度样本的数量
    3、vth_sample:旋转速度样本的数量
    上面的数量大小都是由计算能力决定的
    4、一般情况下，设置vth_sample高于平移速度。因为旋转比直线更加复杂
    5、一般情况下，y向最大速度设置为0，则没必要在y方向提取速度样本，因为没有可用的样本。
    6、一般情况下，我们设置vx_sample=20,vth_samples=40。

5、sim_granularity 仿真粒度

 sim_granularity是在轨迹上的点之间采取的步长。它意味着要多频繁的检查轨迹上的点（检测它们是否与障碍物相交）。
 较低的值意味着高频率，这需要更多的计算能力。对于turtlebot机器人来说，0.025的默认值是足够的。

6、轨迹得分
DWA Local Planner 的最大目标函数是来获取最佳速度对。目标函数的值依赖于三个组成部分

 1、到目标点的过程
 2、清除障碍物
 3、前进速度

path_distance_bias:本地规划器与全局路径保持一致的权重，值越大，local_planner就会更倾向于跟踪全局路径。
goal_distance_bias:机器人尝试到达目标点的权重，值越大，机器人与全局路径的一致性偏低
occdost_scale:机器人尝试躲避障碍物的权重，值越大，机器人就会陷入困境

在SCITOS G5上，我们设置path_distance_bias为32，goal_distance_bias为20，occdist_sacle为0.02,仿真结果良好。

7、其他参数
目标距离公差：

 yaw_goal_tolerance（double 默认值0.05），实现目标时，偏航、旋转中控制器弧度公差
 xy_goal_tolerance（double,，默认值：0.10），实现目标时，在x y方向的距离公差
 latch_xy_goal_tolerance（bool，默认：false）如果目标公差被锁定，即使在目标公差之前结束，
 机器人到达目标xy位置，它会简单旋转到位
 振荡复位（Oscilation reset）：在通过门口的情况下，机器人可能会来回振荡，是因为本地规划器正在产生
 通过两个相反方向的路径。如果机器人保持振荡，导航堆栈将让机器人尝试恢复行为。
 oscillation_reset_dist（double，默认值：0.05）在振荡标志复位之前，机器人以米为单位行走多远

8、dwa参数的总结：————————————————————

sim_time：机器人的仿真时间，时间越长，局部规划的路线越长，对于计算机的性能也要求要高
sim_granularity：轨迹点之间的步长，越短频率越高，要求的计算机的性能要好
angular_sim_granularity：其粒度检查旋转弧度碰撞
path_distance_bias：本地规划器与全局路径保持一致的权重，值越大，local_planner就会更倾向于跟踪全局路径。
goal_distance_bias：机器人尝试到达目标点的权重，值越大，机器人与全局路径的一致性偏低
occdist_scale：成本函数中障碍物距离的权重
twirling_scale：惩罚在机器人的任何变化标题的权重
stop_time_buffer：机器人必须为一个碰撞的轨迹是在秒视为有效之前停止的时间
oscillation_reset_dist：在摇动标志重置之前，机器人必须行驶的距离，以米为单位
oscillation_reset_angle：角的机器人必须在振荡标志复位，弧度
forward_point_distance：从中心点距离的地方of the机器人在一个额外的得分点，在米
scaling_speed：开始缩放机器人足迹的速度的绝对值
max_scaling_factor：影响机器人足迹的最大因子
use_dwa:用动态窗口法约束采样速度到小窗口 (True)
restore_defaults:恢复到原来的配置 (False)
dwa_local_planner:(DWAPlanner)
vx_sample：x方向上平移速度样本的数量
vy_sample: y方向上平移速度样本的数量
vth_sample:旋转速度样本的数量
yaw_goal_tolerance（double 默认值0.05），实现目标时，偏航、旋转中控制器弧度公差
xy_goal_tolerance（double,，默认值：0.10），实现目标时，在x y方向的距离公差
latch_xy_goal_tolerance（bool，默认：false）如果目标公差被锁定，即使在目标公差之前结束，
机器人到达目标xy位置，它会简单旋转到位
振荡复位（Oscilation reset）：在通过门口的情况下，机器人可能会来回振荡，是因为本地规划器正在产生
通过两个相反方向的路径。如果机器人保持振荡，导航堆栈将让机器人尝试恢复行为。
max_trans_vel：机器人的最大平移速度
min_trans_vel：机器人最小的平移速度
max_vel_x：X轴上的最大速度
min_vel_x：x轴上的最小速度
max_trans_vel：机器人的最大速度
min_trans_vel：机器人的最小速度
max_rot_vel：最大的角速度
min_rot_vel:最小的角速度
acc_lim_x：x轴上的加速度
acc_lim_theta：角速度的加速度

2、Costmap Parameters

Costmap参数对DWA Local Planner是至关重要的，在Ros中有三种类型：

    1、静态地图层：直接给导航stack提供静态的Slam地铁解释
    2、障碍物地图层：2D障碍物和3D障碍物体素层
    3、膨胀层：障碍物膨胀来计算每个2D Costmap单元

1、footprint
足迹是移动基站的轮廓。只要是为了适合机器人对障碍物进行膨胀。一般情况下，我们设置的footprint要稍微大于机器人的实际轮廓

2、膨胀
膨胀是由2个参数所决定的
inflation_radius控制零成本点距离障碍物有多远。
cost_scaling_factor与单元的代价值成反比，设置高值将使衰减更陡峭。

建议costmap的曲线是具有相对较低斜率的曲线，以便最佳路径尽可能远离每侧的障碍物。

在图14中，膨胀半径=0.55，代价比例因子=5.0；在图15中，膨胀半径=1.75，代价比例因子=2.58

inflation_radius决定越大，距离障碍物越远
cost_scaling_factor越大，膨胀点越宽，使得机器人越来越靠中间，远离墙边或障障碍物

3、costmap resolution
全局代价地图精度：保持与提供给导航地图的分辨率相同（激光扫描仪的分辨率），否则在建立图形的时候会出现小的未知点