ROS::导航参数配置详解

ROS::导航

本文不会详谈相关概念，都是干货，请有一定基础再来阅读。

建图

gmapping 实例说明

 
<launch>
  <!-- 雷达话题 -->
  <arg name="scan_topic"  default="scan" />
  <!-- 机器人基座坐标系 -->
  <arg name="base_frame"  default="base_footprint"/>
  <!-- 里程计坐标系 -->
  <arg name="odom_frame"  default="odom_combined"/>

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <param name="base_frame" value="$(arg base_frame)"/>
    <param name="odom_frame" value="$(arg odom_frame)"/>
    <!-- 地图更新频率 deafult:5.0 地图更新的时间间隔(单位：seconds)。更新间隔越小，计算负荷越大。-->
    <param name="map_update_interval" value="0.01"/>
    <!-- 探测最大可用范围，光束可达范围 default:80.0 激光雷达最大可用距离。光束被裁减成这个值。 -->
    <param name="maxUrange" value="4.0"/>
    <!-- 传感器最大范围。如果在传感器距离范围内没有障碍物应该在地图上显示为自由空进。maxUrange<真实传感器最大距离范围<=maxRange -->
    <param name="maxRange" value="5.0"/>
    <!-- endpoint匹配标准差 default:0.05 用作结束点匹配。-->
    <param name="sigma" value="0.05"/>
    <!-- 用于查找对应的kernel size default:1 在内核内寻找一个对应。-->
    <param name="kernelSize" value="3"/>
    <!-- 平移优化步长 default:0.05  平移的最优步长。-->
    <param name="lstep" value="0.05"/>
    <!-- 旋转优化步长 default:0.05  旋转的最优步长。-->
    <param name="astep" value="0.05"/>
    <!-- 扫描匹配迭代步数 default:5 扫描匹配的迭代次数。-->
    <param name="iterations" value="5"/>
    <!-- 用于扫描匹配概率的激光标准差 default:0.075 波束的sigma，用来计算似然估计。 -->
    <param name="lsigma" value="0.075"/>
    <!-- 似然估计为平滑采样影响使用的gain default3.0 评估似然的增益，用来平滑重采样的影响。 -->
    <param name="ogain" value="3.0"/>
    <!-- 每次扫描跳过的光束数 default:0 -->
    <param name="lskip" value="0"/>
    <!-- 为获得好的扫描匹配输出结果，用于避免在大空间范围使用有限距离的激光扫描仪（如5m）
         出现的jumping pose estimates问题。当Scores高达600,如果出现了该问题可以考虑设置50 default:0-->
    <param name="minimumScore" value="30"/>
    <!-- 平移时里程误差作为平均函数(rho/rho) default:0.1 -->
    <param name="srr" value="0.01"/>
    <!-- 平移时里程误差作为旋转函数(rho/theta) default:0.2 -->
    <param name="srt" value="0.02"/>
    <!-- 旋转时的里程误差作为平移函数(theta/rho) default:0.1 -->
    <param name="str" value="0.01"/>
    <!-- 旋转时的里程误差作为旋转函数(theta/theta) default:0.2 -->
    <param name="stt" value="0.02"/>
    <!-- 机器人平移这么远，处理一次扫描。 -->
    <param name="linearUpdate" value="0.05"/>
    <!-- 机器人旋转这个角度，处理一次扫描。 -->
    <param name="angularUpdate" value="0.0436"/>
    <!-- 如果最新扫描处理比更新慢，则处理1次扫描。该值为负数时候关闭基于时间的更新 default:-1.0 -->
    <param name="temporalUpdate" value="-1.0"/>
    <!-- 基于重采样门限的Neff default: 0.5 基于重采样阈值的Neff。-->
    <param name="resampleThreshold" value="0.5"/>
    <!-- 滤波器中粒子数目 default:30-->
    <param name="particles" value="8"/>
    <!-- 地图初始尺寸 default -100 -->
    <param name="xmin" value="-1.0"/>
    <!-- 地图初始尺寸 default: -100 -->
    <param name="ymin" value="-1.0"/>
    <!-- 地图初始尺寸 default: 100 -->
    <param name="xmax" value="1.0"/>
    <!-- 地图初始尺寸 default: 100 -->
    <param name="ymax" value="1.0"/>
    <!-- 地图分辨率 default:0.05 -->
    <param name="delta" value="0.05"/>
    <!-- 于似然计算的平移采样距离  default: 0.01-->
    <param name="llsamplerange" value="0.01"/>
    <!-- 用于似然计算的平移采样步长 default: 0.01 -->
    <param name="llsamplestep" value="0.01"/>
    <!-- 用于似然计算的角度采样距离 default:0.005 -->
    <param name="lasamplerange" value="0.005"/>
    <!-- 用于似然计算的角度采样步长 default:0.005 -->
    <param name="lasamplestep" value="0.005"/>
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>
</launch>
  

了解一下

Gmapping是一个基于2D激光雷达使用RBPF（Rao-Blackwellized Particle Filters）算法完成二维栅格地图构建的SLAM算法。

优点：gmapping可以实时构建室内环境地图，在小场景中计算量少，且地图精度较高，对激光雷达扫描频率要求较低。

缺点：随着环境的增大，构建地图所需的内存和计算量就会变得巨大，所以gmapping不适合大场景构图。一个直观的感受是，对于200x200米的范围，如果栅格分辨率是5cm，每个栅格占用一个字节内存，那么每个粒子携带的地图都要16M的内存，如果是100粒子就是1.6G内存。

FastSLAM算法中的机器人轨迹估计问题使用的粒子滤波方法。由于使用的是粒子滤波，将不可避免的带来两个问题。第一个问题，当环境大或者机器人里程计误差大时，需要更多的粒子才能得到较好的估计，这时将造成内存爆炸；第二个问题，粒子滤波避免不了使用重采样，以确保当前粒子的有效性，然而重采样带来的问题就是粒子耗散，粒子多样性的丢失。由于这两个问题出现，导致FastSLAM算法理论上可行实际上却不能实现。针对以上问题gmapping提出了两种针对性的解决方法。也就是说gmapping是基于FastSLAM算法将RBPF方法变成了现实。

定位

AMCL(自适应蒙特卡洛定位)实例说明

<launch>

    <!-- 使用map的topic而不是map文件-->
    <arg name="use_map_topic" default="false"/>
    <!-- 不可以使用PointCloud，只能使用LaserScan -->
    <arg name="scan_topic" default="scan"/>
    <arg name="map_topic"  default="/map"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
      <!-- 当设置为true时，AMCL将订阅地图主题，而不是进行服务调用以接收其地图。 -->
      <param name="use_map_topic" value="$(arg use_map_topic)"/>
      <!-- 使用rtabmap分布的Map，连接TF树 -->
      <remap from="/map" to="/$(arg map_topic)"/>
      <!-- 使用哪个模型，“diff”，“omni”，“diff-corrected”或“omni-corrected”。 -->
      <param name="odom_model_type" value="omni-corrected"/>
      
      <!--===================== 总体过滤器参数 ================================-->
      <!-- 最小允许的颗粒数。 default: 100 -->
      <param name="min_particles" value="500"/>
      <!-- 最大允许的颗粒数。 default: 5000 -->
      <param name="max_particles" value="2000"/>
      <!-- 真实分布和估计分布之间的最大误差。default: 0.01 -->
      <param name="kld_err" value="0.05"/>
      <!-- 1-p）的上标准正常分位数，其中p是估计的失谐上的误差将小于kld_err的概率。 default: 0.99 -->
      <param name="kld_z" value="0.99"/>
      <!-- 执行过滤器更新之前需要执行平移运动。default: 0.2m -->
      <param name="update_min_d" value="0.25"/>
      <!-- 执行过滤器更新之前需要执行旋转运动。default: π/ 6.0 radians -->
      <param name="update_min_a" value="0.2"/>
      <!-- 重新采样之前所需的过滤器更新数。default: 2 -->
      <param name="resample_interval" value="1"/>
      <!-- 将发布的变换后期化的时间，以指示此变换在未来有效。default: 0.1s -->
      <param name="transform_tolerance" value="1.0"/>
      <!-- 慢平均权重滤波器的指数衰减率，用于决定何时通过添加随机姿态来恢复。良好的值可能为0.001。default: 0.0(禁用) -->
      <param name="recovery_alpha_slow" value="0.0"/>
      <!-- 快速平均权重滤波器的指数衰减率，用于决定何时通过添加随机姿态来恢复。好的值可能为0.1。default: 0.0(禁用) -->
      <param name="recovery_alpha_fast" value="0.0"/>
      <!-- 初始姿态均值（x），用于初始化具有高斯分布的滤波器。default: 0.0m -->
      <param name="initial_pose_x" value="0.0"/>
      <!-- 初始姿态平均值（y），用于初始化具有高斯分布的滤波器。 default: 0.0m -->
      <param name="initial_pose_y" value="0.0"/>
      <!-- 初始姿态平均（偏航），用于初始化具有高斯分布的滤波器。 default: 0.0弧度 -->
      <param name="initial_pose_a" value="0.0"/>
      <!-- 发布可视化扫描和路径的最大速率（Hz），禁用-1.0。 default: -1.0 Hz -->
      <param name="gui_publish_rate" value="10.0"/>
      
      <!--===================== 激光模型参数 ================================-->
      <!-- 在更新过滤器时要在每次扫描中使用多少均匀间隔的光束。 default: 30 -->
      <param name="laser_max_beams" value="60"/>
      <!-- 要考虑的最大扫描范围; -1.0将导致使用激光器报告的最大范围。 default: -1.0 -->
      <param name="laser_max_range" value="12.0"/>
      <!-- 模型的z_hit部分的混合重量。 default: 0.95 -->
      <param name="laser_z_hit" value="0.5"/>
      <!-- 模型的z_short部分的混合重量。 default: 0.1 -->
      <param name="laser_z_short" value="0.05"/>
      <!-- 模型的z_max部分的混合物重量。 default: 0.05 -->
      <param name="laser_z_max" value="0.05"/>
      <!-- 模型的z_rand部分的混合重量。 default: 0.05 -->
      <param name="laser_z_rand" value="0.5"/>
      <!-- 在模型的z_hit部分中使用的高斯模型的标准偏差。 default: 0.2米 -->
      <param name="laser_sigma_hit" value="0.2"/>
      <!-- 模型的z_short部分的指数衰减参数。 default: 0.1 -->
      <param name="laser_lambda_short" value="0.1"/>
      <!-- 使用哪个模型，beam，likelihood_field或likelihood_field_prob（与likelihood_field相同，但包含beamkip功能，如果已启用）。 default: “likelihood_field” -->
      <param name="laser_model_type" value="likelihood_field"/>
      <!-- 在地图上做障碍物充气的最大距离，用于likelihood_field模型。 default: 2.0m -->
      <param name="laser_likelihood_max_dist" value="2.0"/>
      
      <!--===================== 里程计模型参数 ================================-->
      <!-- 根据机器人运动的旋转分量指定里程表旋转估计中的预期噪声。default:0.2 -->
      <param name="odom_alpha1" value="0.2"/>
      <!-- 根据机器人运动的平移分量，指定里程表旋转估计中的预期噪声。default:0.2 -->
      <param name="odom_alpha2" value="0.2"/>
      <!-- 从机器人运动的平移分量中指定odometry的翻译估计中的预期噪声。 default:0.2 -->
      <param name="odom_alpha3" value="0.2"/>
      <!-- 根据机器人运动的旋转分量指定odometry的翻译估计中的预期噪声。 default:0.2 -->
      <param name="odom_alpha4" value="0.2"/>
      <!-- 与翻译相关的噪声参数（仅在模型为“omni”时使用）。 default:0.2 -->
      <param name="odom_alpha5" value="0.1"/>
      <!-- 用于odometry的帧。 default:odom -->
      <param name="odom_frame_id" value="odom_combined"/>
      <!-- 由本地化系统发布的坐标系的名称 default:map -->
      <param name="global_frame_id" value="map"/>

      <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

AMCL是机器人在2D中移动的概率定位系统。它实现了自适应（或KLD采样）蒙特卡罗定位方法，其使用粒子滤波器来针对已知的地图跟踪机器人的姿势。

关于粒子滤波

简单来说，在机器人定位问题中，我们想要估计机器人的位置和姿态。最初，我们完全不知道机器人在哪，那就索性假设机器人以同等的概率出现在地图上的任意一个位置。比如，假如地图是整个中国，那么机器人就等可能地出现在北京、上海、广州、哈尔滨等地。于是我们就可以用一个粒子代表一个机器人可能出现的位置。现在，机器人说话了，它说它感觉特别冷，雪落在了它洁白的脖子上。于是，我当即排除了长江以南的城市，南方怎么可能下雪呢！机器人系上围脖，继续向前走。没走几步，它又抱怨道，“今天空气质量可真不怎么样，我的双目都要失明了。”显然，它是遇到了雾霾天，这种天气在北方某帝都倒是挺常见的。它点亮了IR主动光探测，雾霾天上路多加小心总没有错。转眼间，机器人来到一个庞大的建筑物面前，这里人声鼎沸，还有遍地的商贩在叫卖着不知什么东西。它借助自身廉价的激光雷达小心翼翼地在人群中穿梭。突然，它若有所思地停了下来，似乎发现了一个美丽的秘密。虽然外面寒冬凌冽，这里却如春天般富有生机，到处洋溢着绿色的海洋。人声此起彼伏，它依稀听出了五个字，“国安是冠军”…

在上面的例子中，“我”作为机器人的大脑，根据机器人的感受，可以得出如下推理。机器人发现下雪了，那么可以确定机器人应该在北方的某个城市。接着，机器人遇到了雾霾天，那么说明该城市的空气质量很差，这就进一步把搜索范围缩小到了某几个重点空气污染城市。最后，机器人听到的五个字“国安是冠军”，彻底让我锁定了它所在的城市——“北京”。

这就是一个形象的粒子滤波案例。机器人不断地通过运动、观测的方式，获取周围环境信息，逐步降低自身位置的不确定度，最终得到准确的定位结果。

从理论上看粒子滤波。

amcl接收基于激光的地图，激光扫描和变换消息，并输出姿态估计。在启动时，amcl根据提供的参数初始化其粒子滤波器。注意，由于默认值，如果没有设置参数，则初始过滤器状态将是以（0,0,0）为中心的中等大小的粒子云。

订阅主题

（1）scan （sensor_msgs / LaserScan）
激光扫描数据。

（2））tf（tf / tfMessage）
tf转换。

（3）initialpose（geometry_msgs / PoseWithCovarianceStamped）
用于（重新）初始化粒子滤波器的平均值和协方差。

（4）map（nav_msgs / OccupancyGrid）
当设置了use_map_topic参数时，AMCL订阅此主题以检索用于基于激光的本地化的映射。

发布主题

（1）amcl_pose（geometry_msgs / PoseWithCovarianceStamped）
机器人在地图上的估计姿态，具有协方差。

（2）particlecloud（geometry_msgs / PoseArray）
由过滤器维护的姿态估计集合。

（3）tf（tf / tfMessage）
从odom发布的tf变换（可通过〜odom_frame_id参数被重新映射），以映射。

导航 路径规划

<launch>

  <rosparam file="$(find robot)/param_common/local_costmap_params.yaml"      command="load" ns="move_base"/>
  <rosparam file="$(find robot)/param_common/global_costmap_params.yaml"     command="load" ns="move_base"/>
  <rosparam file="$(find robot)/param_common/move_base_params.yaml"          command="load" ns="move_base"/>
  <rosparam file="$(find robot)/param_common/base_global_planner_param.yaml" command="load" ns="move_base"/>
  <rosparam file="$(find robot)/param_common/teb_local_planner_params.yaml"  command="load" ns="move_base"/>

  <arg name="car_mode"  default="mini_mec"  doc=""/>

  <group if="$(eval car_mode == 'mini_mec')">
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find robot)/costmap_common_params/param_mini_mec/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find robot)/costmap_common_params/param_mini_mec/costmap_common_params.yaml" command="load" ns="local_costmap" />
  </node>
  </group>
</launch>

加载了costmap_common_params.yaml到global_costmap和local_costmap两个命名空间中，因为该配置文件是一个通用的代价地图配置参数，即local_costmap和global_costmap都需要配置的参数。

costmap_common_params.yaml

max_obstacle_height: 2.0 #传感器读数的最大有效高度，单位为 meters; 
                         #通常设置为略高于机器人的实际高度，高度是指包含机械臂打直情况下的最大高度。

# robot_radius: 0.4  # 如果机器人圆形的，注释下面的一行，开启这个



footprint: [[-0.133, -0.125], [-0.133, 0.125],[0.133,0.125],[0.133, -0.125]] #  [[x0, y0], [x1, y1], ... [xn, yn]]  
                                                                             # 当机器人非圆形时，先找机器人的旋转中心，即两个轮的中心点设置成(0,0)，
                                                                             # 然后确定机器人摆放方向，x,y为每个robot几何型的每条边的每个顶点。
                                                                             # 将所有顶点都列到其中。就完成了robot的footprint。

#map_type: voxel # 地图类型，这里为voxel(体素地图)。
                 # 另一种地图类型为costmap(代价地图)。
                 # 这两者之间的区别是前者是世界的3D表示，后者为世界的2D表示。

obstacle_layer:  # 障碍物层参数

  enabled:              true   #使能障碍层
  
  max_obstacle_height:  2.0 #传感器读数的最大有效高度（单位：m）。 通常设置为略高于机器人的高度。 此参数设置为大于全局max_obstacle_height参数的值将会失效。
                            #设置为小于全局max_obstacle_height的值将过滤掉传感器上大于该高度以的点。 

  min_obstacle_height:  0.0

  
  #origin_z:             0.0 # z原点，单位为米，仅对voxel地图

  #z_resolution:         0.2 #z分辨率，单位meters/cell

  #z_voxels:             2 #每个垂直列中的体素数目，ROS Nav功能包的默认值为10。请参考https://github.com/teddyluo/ROSNavGuide-Chinese

  #unknown_threshold:    15 #当整列的voxel是“已知”(``known’’)的时候，含有的未知单元(“unknown”)的最大数量

  #mark_threshold:       0 # 整列voxel是“自由”(“free”)的时候，含有的已标记的cell(“marked”)的最大数目。

  combination_method:   1 #只能设置为0或1，用来更新地图上的代价值，一般设置为1;


  track_unknown_space:  true #如果设置为false，那么地图上代价值就只分为致命碰撞和自由区域两种，如果设置为true，那么就分为致命碰撞，自由区域和未知区域三种。
                             #意思是说假如该参数设置为false的话，就意味着地图上的未知区域也会被认为是可以自由移动的区域，这样在进行全局路径规划时，
                             #可以把一些未探索的未知区域也来参与到路径规划，如果你需要这样的话就将该参数设置为false。不过一般情况未探索的区域不应该当作可以自由移动的区域，
                             #因此一般将该参数设置为true;

  obstacle_range: 2.5 #这些参数设置了代价地图中的障碍物信息的阈值。 "obstacle_range" 参数确定最大范围传感器读数
                      #这将导致障碍物被放入代价地图中。在这里，我们把它设置在2.5米，这意味着机器人只会更新其地图包含距离移动基座2.5米以内的障碍物的信息。

  raytrace_range: 3.0 #“raytrace_range”参数确定了用于清除指定范围外的空间。
                      #将其设置为3.0米，这意味着机器人将尝试清除3米外的空间，在代价地图中清除3米外的障碍物。

  #origin_z: 0.0 #z原点，单位为米，仅对voxel地图

  #z_resolution: 0.2

  #z_voxels: 2
  
  publish_voxel_map: false #是否发布底层的体素栅格地图，其主要用于可视化。

  observation_sources:  scan # 设置导航中所使用的传感器，这里可以用逗号形式来区分开很多个传感器，例如激光雷达，碰撞传感器，超声波传感器等，我们这里只设置了激光雷达;
                             

  scan: #观察源之一：激光数据。定义了：观察源的数据类型，发布话题，标记和添加障碍物
    data_type: LaserScan #观察源的数据类型：激光扫描
    topic: "/scan" #发布话题为scan
    marking: true #启用标记障碍物功能
    clearing: true #启用清除障碍物功能
                   #关于Marking and Clearing：
                   # marking和clearing参数用来表示是否需要使用传感器的实时信息来添加或清除代价地图中的障碍物信息）
                   # 代价地图自动订阅传感器主题并自动更新。
                   # 每个传感器用于标记操作（将障碍物信息插入到代价地图中），清除操作（从代价地图中删除障碍物信息）或两者操作都执行。
                   # 如果使用的是体素层，每一列上的障碍信息需要先进行投影转化成二维之后才能放入代价地图中。
    expected_update_rate: 0

    #min_obstacle_height: 0.25 #传感器最低有效读数，以米为单位。
                               #通常设置为地面高度，但可以根据传感器的噪声模型设置为更高或更低。
    #max_obstacle_height: 0.35 #传感器读数的最大有效高度，以米为单位。
                               #通常设置为略大于机器人的最大高度。设置为大于全局的max_obstacle_height的值会失效。
                               #设置为小于全局max_obstacle_height将从传感器上过滤掉该高度以上的点。



inflation_layer: #膨胀层参数
  enabled:              true #使能膨胀层

  cost_scaling_factor:  5.0  #在膨胀期间应用于代价值的尺度因子。
                             #默认值：10。对在内接半径之外的cells、以及在内接半径之内的cells这两种不同的cells, 代价函数的计算公式为：
                             #exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)

  inflation_radius:     0.2  #机器人膨胀半径，比如设置为0.3,意味着规划的路径距离0.3米以上，这个参数理论上越大越安全
                             #但是会导致无法穿过狭窄的地方

static_layer: #静态地图层
  enabled:              true  #启用静态地图
  map_topic:            "/map"

• footprint 参数补充说明

global_costmap_params.yaml

global_costmap:
  global_frame: map #全局代价地图需要在哪个坐标系下运行;
  robot_base_frame: base_footprint #在全局代价地图中机器人本体的基坐标系，就是机器人上的根坐标系。通过global_frame和robot_base_frame就可以计算两个坐标系之间的变换，得知机器人在全局坐标系中的坐标了。
  update_frequency: 1.5 #参数决定了代价地图更新的频率（以Hz为单位）。根据传感器数据，全局地图更新的频率，这个数值越大，你的计算机的CPU负担会越重，特别对于全局地图，通常设定一个相对较小、在1.0到5.0之间的值。
  publish_frequency: 1.0 #参数决定代价地图发布可视化信息的速率（以Hz为单位）。
  #static_map: true #参数确定是否由map_server提供的地图服务来进行代价地图的初始化。如果您没有使用现有的地图或地图服务器，请将static_map参数设置为false。当本地地图需要根据传感器数据动态更新的时候，我们通常会把这个参数设为false。
  transform_tolerance: 1 #坐标系间的转换可以忍受的最大延时
  plugins: #在global_costmap中使用下面三个插件来融合三个不同图层，分别是static_layer、obstacle_layer和inflation_layer，合成一个master_layer来进行全局路径规划。
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

move_base_params.yaml

shutdown_costmaps: false  #当move_base在不活动状态时,是否关掉costmap.

controller_frequency: 10.0  #向底盘控制移动话题cmd_vel发送命令的频率.
controller_patience: 3.0 #在空间清理操作执行前,控制器花多长时间等有效控制下发
 
planner_frequency: 1.0  #全局规划操作的执行频率.如果设置为0.0,则全局规划器仅
                        #在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
planner_patience: 3.0  #在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.

oscillation_timeout: 10.0  #执行修复机制前,允许振荡的时长.
oscillation_distance: 0.02  #来回运动在多大距离以上不会被认为是振荡.

#全局路径规划器
base_global_planner: "global_planner/GlobalPlanner" #指定用于move_base的全局规划器插件名称.
#base_global_planner: "navfn/NavfnROS" #指定用于move_base的局部规划器插件名称.
#base_global_planner: "global_planner/GlobalPlanner"
#base_global_planner: "carrot_planner/CarrotPlanner"

#局部路径规划器的调用在文件【teb_local_planner.launch】、【dwa_local_planner.launch】对应文件内自动调用，该部分可以忽略
base_local_planner: "teb_local_planner/TebLocalPlannerROS" #指定用于move_base的局部规划器插件名称.
#base_local_planner: "dwa_local_planner/DWAPlannerROS" #指定用于move_base的全局规划器插件名称.


max_planning_retries: 1  #最大规划路径的重复次数，-1表示无限次

recovery_behavior_enabled: false  #是否启动恢复机制
clearing_rotation_allowed: false  #是否启动旋转的恢复，必须是recovery_behavior_enabled在使能的基础上才能生效


recovery_behaviors:  # 自恢复行为
  - name: 'conservative_reset'  
    type: 'clear_costmap_recovery/ClearCostmapRecovery'  
  #- name: 'aggressive_reset'
  #  type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'super_reset'
  #  type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'clearing_rotation'
    type: 'rotate_recovery/RotateRecovery'  
  #- name: 'move_slow_and_clear'
    #type: 'move_slow_and_clear/MoveSlowAndClear'

#保守清除，用户指定区域之外的障碍物将从机器人地图中清除
conservative_reset:  
  reset_distance: 1.0  
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]  
#保守清除后，如果周围障碍物允许，机器人将进行原地旋转以清理空间

#保守清除失败，积极清除，清除指定区域之外的所有障碍物，然后进行旋转
aggressive_reset:  
  reset_distance: 3.0  
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]
#积极清除也失败后，放弃规划路径

#可能是更进一步的清除，wiki未找到相关资料
super_reset:  
  reset_distance: 5.0  
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]

#另一种恢复行为，需要注意该行为只能与具有动态设置速度限制功能的局部路径规划器适配，例如dwa
move_slow_and_clear:  
  clearing_distance: 0.5  #与小车距离0.5内的障碍物会被清除
  limited_trans_speed: 0.1  
  limited_rot_speed: 0.4  
  limited_distance: 0.3  

首先，用户指定区域的障碍物地图将会被清除， 其次， 如果可行的话， 机器人将会旋转来清除外部空间，如果这也失败的话， 机器人将会积极地旋转运动来清除方形区域外的障碍物区域， 如果这都失败的话， 机器人将会通知move_base放弃该导航任务。自恢复行为可以通过 recovery_behaviors参数来进行配置， 或者使用recovery_behavior_enabled参数来开启和关闭自恢复行为。

base_global_planner_param.yaml

GlobalPlanner:      
  old_navfn_behavior: false    #是否使用旧版本导航功能，默认false                 
  use_quadratic: true          #是否使用二次逼近的趋势，默认为true，否则使用简单的线性方法 二次近似函数，false使用更简单的计算方式节省硬件资源（否的话会扩展周边4个点，是扩展周边八个）
  use_dijkstra: true           #是否使用D*全局规划算法，默认为true，如果为false，则使用A*算法                 
  use_grid_path: false         #是否沿着网格规划路径，默认为false，false时使用最速下降法计算路径                
  
  allow_unknown:  true         #是否允许规划路径穿越未知区域                 
                                               
  planner_window_x: 0.0       # default 0.0 指定可选窗口的x,y大小以限定规划器的工作空间(程序中未用到)
  planner_window_y: 0.0       # default 0.0
  default_tolerance: 0.0      # default 0.0 允许目标点的误差范围

须知:全局规划器与本地规划器种类 这里只对teb进行说明

global_planner:

• A*
• Dijstra
• prm
• 人工势场
• 单元分解
• 快速搜索树（RRT)

local_planner:

• base_local_planner
• dwa_local_planner
• teb_local_planner
• eband_local_planner
• asr_ftc_local_planner
• dwb_local_planner

动态窗口法(DWA)

算法简介：
DWA算法全称为dynamic window approach，其原理主要是在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的速度驱动机器人运动。

动态窗口法与ROS默认局部路径规划算法TrajectoryPlanner类似，不同之处在于对机器人控制空间的采样：在给定机器人的加速度极限的情况下，TrajectoryPlanner在整个前向模拟周期内从可实现的速度集合中进行采样，而DWA在给定机器人的加速度极限的情况下仅针对一个模拟步骤从可实现的速度集合中进行采样。在实际使用过程中，TrajectoryPlanner和DWA算法效果类似，但是DWA算法更加高效，占用内存更少，所以在这两种算法中间一般直接选择DWA。

DWA算法在ROS中为dwa_local_planner
DWA算法分析参考博文DWA算法分析及实现

优点：

计算复杂度低：考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小
可以实现避障：可以实时避障，但是避障效果一般
适用与差分和全向车模

缺点：

前瞻性不足：只模拟并评价了下一步，如在机器人前段遇见“C”字形障碍时，不能很好的避障
动态避障效果差： 模拟运动轨迹断，动态避障效果差
非全局最优路径： 每次都选择下一步的最佳路径，而非全局最优路径
不适用于阿克曼模型车模

时间弹性带(TEB)

算法简介：
TEB全称为Time Elastic Band，算法浅析参考博文TEB浅析，文中关于eletic band（橡皮筋）的定义：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。关于time eletic band的简述：起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time，即为Timed-Elastic-Band算法。通过此方法可以把问题描述为一个多目标优化问题，通过构建超图(hyper-graph)，使用g2o框架中的图优化来求解。

优点：

适用于各种常见车模：如差分、全向、阿克曼模型
有很强的前瞻性： 对前方一段轨迹进行优化
动态避障： 对动态障碍有较好的避障效果，可直接使用其封装好障碍类Obstacle

缺点：

计算复杂度较大：可通过牺牲预测距离来降低复杂度
速度和角度波动较大、控制不稳定： 源码中是通过两状态之间的距离和角度差及时间差来计算该控制周期内的速度和角速度，使得在控制过程中速度和角度波动较大，如下图所示。在计算资源足够的情况下，提高控制频率可以改善此现象。
非全局最优： 但是好于DWA

改进策略

针对其控制不稳定问题，主要原因是每个控制周期内，速度变化较大。一种方法是提高控制频率，另一种方法是使用优化的方法，即修改TEB算法的评价函数，把每次速度和角度的变化量除以时间再乘一个代价系数。

asr_ftc_local_planner意为“跟随萝卜”的算法，以尽可能地贴合全局路径。

当发送一个新目标点后，算法可划分为三个阶段：
将机器人的转向目标点朝向
走向目标
在目标点将机器人的朝向旋转到位

主要优势

快速驾驶：机器人尽可能达到最大速度行驶。
参数很少：只需设置11个参数。
很少有剧烈转换的情况：机器人尽可能以最大速度往前开。
小房间情况：机器人紧密跟随全局路径，不像dwa_local_planner那样存在绕目标点循环的问题。所以它可以在小房间中无冲突行驶。
通过全局路径在前进中排除障碍：保证在障碍物附近找到路径。
处理时间短

缺点

仅适用于圆形的机器人。
只适用于差动驱动的机器人。
仅有向前走（没有后退机制）。
可见它是一种很简单的路径跟随规划器，适用于AGV这种在固定位置有预设动作的机器人。

teb_local_planner_params.yaml

TebLocalPlannerROS:

 odom_topic: /odom
 map_frame: odom_combined

 #Trajectory
 teb_autosize: True #优化期间允许改变轨迹的时域长度 default:True
 dt_ref: 0.45 #局部路径规划的解析度# minimum 0.01 default:0.3  0.01~1.0
 dt_hysteresis: 0.1 #允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右 default:0.1  0.002~0.5
 global_plan_overwrite_orientation: False #覆盖全局路径中局部路径点的朝向 default:True
 #allow_init_with_backwards_motion:False #允许在开始时想后退来执行轨迹 default:False
 max_global_plan_lookahead_dist: 3.0 #考虑优化的全局计划子集的最大长度 （累积欧几里得距离）（如果为0或负数：禁用；长度也受本地Costmap大小的限制） default:3.0  0.0~50.0
 feasibility_check_no_poses: 5 #检测位姿可到达的时间间隔 default:5 0~50

 # Robot
 max_vel_x: 0.5 #最大x前向速度
 max_vel_y: 0.3 #最大y前向速度，非全向移动小车需要设置为0
 max_vel_x_backwards: 0.5 #最大后退速度
 max_vel_theta: 1.5  #最大转向角速度
 acc_lim_x: 0.2  #最大x向加速度
 acc_lim_y: 0.2  #最大y向加速度，非全向移动小车需要设置为0
 acc_lim_theta: 0.3  #最大角加速度

 #阿克曼小车参数，非阿克曼小车设置为0
 min_turning_radius: 0.0   #车类机器人的最小转弯半径 default:0.0   0.0~50.0

 wheelbase: 0.0  #驱动轴和转向轴之间的距离（仅适用于启用了“Cmd_angle_而不是_rotvel”的Carlike机器人）；对于后轮式机器人，该值可能为负 default:1.0  -10.0~10.0
 #wheelbase: 0.143 #for mini_akm
 #wheelbase: 0.320 #for senior_akm
 #wheelbase: 0.503 #for top_akm_bs
 #wheelbase: 0.549 #for top_akm_dl

 cmd_angle_instead_rotvel: False #将收到的角速度消息转换为 操作上的角度变化。 default:False
                                 #true则cmd_vel/angular/z内的数据是舵机角度

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 默认“point”类型
  type: "polygon"   #多边形类型for akm/mec，默认”point”。

  #麦轮
  vertices: [[-0.133, -0.125], [-0.133, 0.125],[0.133,0.125],[0.133, -0.125]] #多边形端点坐标 for mini_mec

  #全向轮
  #radius: 0.12 #for mini_omni

 xy_goal_tolerance: 0.2   #目标 xy 偏移容忍度 default:0.2 minimum 0.001 maximum 0.2
 yaw_goal_tolerance: 0.1 #目标 角度 偏移容忍度 default:0.1  minimum 0.001 maximum 0.1
 free_goal_vel: False  #允许机器人以最大速度驶向目的地 default:False
 complete_global_plan: True
 
 # Obstacles
 min_obstacle_dist: 0.1 #和障碍物最小距离 default:0.5  0.0~10.0
 include_costmap_obstacles: True  #是否将动态障碍物预测为速度模型，costmap 中的障碍物是否被直接考虑 default:True  
 costmap_obstacles_behind_robot_dist: 1.5  #限制机器人后方规划时考虑的局部成本地图障碍物 default:1.5  0.0~20.0
 obstacle_poses_affected: 15   #障碍物姿态受影响0~30 default:30  0~200
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5
 include_dynamic_obstacles: True   #是否将动态障碍物预测为速度模型 default False
 dynamic_obstacle_inflation_dist: 0.6 # 动态障碍物的膨胀范围 default:0.6  0.0~15.0

 # Optimization  
 no_inner_iterations: 5  #被外循环调用后内循环执行优化次数 default:5  1~100
 no_outer_iterations: 4  #执行的外循环的优化次数 default:4  1~100
 optimization_activate: True #激活优化 default:True
 optimization_verbose: False # 打印优化过程详情
 penalty_epsilon: 0.1 #对于硬约束近似，在惩罚函数中添加安全范围 default:0.1  0.0~1.0
 obstacle_cost_exponent: 4
 weight_max_vel_x: 2  #最大x速度权重 default:2.0  0.0~1000.0
 weight_max_vel_theta: 1 #最大速度权重 default:1.0  0.0~1000.0
 weight_acc_lim_x: 1  #最大x 加速度权重 default:1.0  0.0~1000.0
 weight_acc_lim_theta: 1  #最大角速度权重 default:1.0  0.0~1000.0
 weight_kinematics_nh: 1000 #满足非完整运动学的最优权 default: 1000.0  0~10000.0
 weight_kinematics_forward_drive: 1   #优化过程中，迫使机器人只选择前进方向，差速轮适用 default:1.0  0~1000.0
 weight_kinematics_turning_radius: 1  #优化过程中，车型机器人的最小转弯半径的权重。 default:1.0   0.0~1000
 weight_optimaltime: 1 # must be > 0  #优化过程中，基于轨迹的时间上的权重 default:1.0  0~1000
 weight_shortest_path: 0 #
 weight_obstacle: 100
 weight_inflation: 0.2   #优化过程中， 膨胀区的权重 default:0.1  0.0~10.0
 weight_dynamic_obstacle: 10 # not in use yet 优化过程中，和动态障碍物最小距离的权重 default:50.0  0.0~1000
 weight_dynamic_obstacle_inflation: 0.2  #优化过程中，和动态障碍物膨胀区的权重  default 1.0  0.0~1000
 weight_viapoint: 1  #优化过程中，和全局路径采样点距离的权重  default:1.0  0.0 1000
 weight_adapt_factor: 2  #default 2.0  1.0~100

 # Homotopy Class Planner
 enable_homotopy_class_planning: False 
 enable_multithreading: True #允许多线程并行处理 default: True
 max_number_classes: 4 # 允许的线程数 default:5 1~100
 selection_cost_hysteresis: 1.0 
 selection_prefer_initial_plan: 0.95
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False

 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_heading_threshold: 0.45
 switching_blocking_period: 0.0
 viapoints_all_candidates: True
 delete_detours_backwards: True
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False
 visualize_with_time_as_z_axis_scale: False

 # Recovery
 
 shrink_horizon_backup: True #当规划器检测到系统异常，允许缩小时域规划范围。default True
 shrink_horizon_min_duration: 10
 oscillation_recovery: False  #尝试检测和解决振荡 default:True
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10

参数参考:
https://blog.csdn.net/Fourier_Legend/article/details/89398485

dwa_local_planner_params.yaml

DWAPlannerROS:
# Robot Configuration Parameters 
  max_vel_x: 0.45  #x方向最大线速度绝对值，单位:米/秒
  min_vel_x: 0 #x方向最小线速度绝对值，负数代表可后退，单位:米/秒
  max_vel_y: 0.0  # #y方向最大线速度绝对值，单位:米/秒。差分驱动机器人为0    
  min_vel_y: 0.0  # #y方向最小线速度绝对值，单位:米/秒。差分驱动机器人为0   
  max_vel_trans: 0.5 #   #机器人最大平移速度的绝对值，单位为 m/s
  min_vel_trans: 0.1  # #机器人最小平移速度的绝对值，单位为 m/s 不可为零  
  trans_stopped_vel: 0.1  #机器人被认属于“停止”状态时的平移速度。如果机器人的速度低于该值，则认为机器人已停止。单位为 m/s
  max_vel_theta: 0.7  #机器人的最大旋转角速度的绝对值，单位为 rad/s 
  min_vel_theta: 0.3  # 器人的最小旋转角速度的绝对值，单位为 rad/s
  theta_stopped_vel : 0.4 #机器人被认属于“停止”状态时的旋转速度。单位为 rad/s  
  acc_lim_x: 0.5 # 机器人在x方向的极限加速度，单位为 meters/sec^2
  acc_lim_theta: 3.5 #机器人的极限旋转加速度，单位为 rad/sec^2
  acc_lim_y: 0.0      #  机器人在y方向的极限加速度，对于差分机器人来说当然是0
# Goal Tolerance Parameters目标距离公差参数
  yaw_goal_tolerance: 0.15 #到达目标点时，控制器在偏航/旋转时的弧度容差(tolerance)。即：到达目标点时偏行角允许的误差，单位弧度
  xy_goal_tolerance: 0.2  # 到到目标点时，控制器在x和y方向上的容差（tolerence）（米）。即：到达目标点时,在xy平面内与目标点的距离误差
  # latch_xy_goal_tolerance: false #设置为true时表示：如果到达容错距离内,机器人就会原地旋转；即使转动是会跑出容错距离外。

# Forward Simulation Parameters前向模拟参数
  sim_time: 1.8      # 前向模拟轨迹的时间，单位为s(seconds) 
  vx_samples: 6       # x方向速度空间的采样点数
  vy_samples: 0       # y方向速度空间采样点数.。差分驱动机器人y方向永远只有1个值（0.0）
  vtheta_samples: 20  # 旋转方向的速度空间采样点数

# Trajectory Scoring Parameters
  path_distance_bias: 100.0      #控制器与给定路径接近程度的权重
  goal_distance_bias: 24.0      #控制器与局部目标点的接近程度的权重，也用于速度控制
  occdist_scale: 0.5            # 控制器躲避障碍物的程度
  forward_point_distance: 0.325 #以机器人为中心，额外放置一个计分点的距离
  stop_time_buffer: 0.1         #机器人在碰撞发生前必须拥有的最少时间量。该时间内所采用的轨迹仍视为有效。即：为防止碰撞,机器人必须提前停止的时间长度
  scaling_speed: 0.25           #开始缩放机器人足迹时的速度的绝对值，单位为m/s。
  max_scaling_factor: 0.2       #最大缩放因子。max_scaling_factor为上式的值的大小。

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05  # 0.05   #机器人必须运动多少米远后才能复位震荡标记(机器人运动多远距离才会重置振荡标记)
  oscillation_reset_angle: 0.05

# Debugging调试参数
  publish_traj_pc : true #将规划的轨迹在RVIZ上进行可视化
  publish_cost_grid_pc: true   #将代价值进行可视化显示
  global_frame_id: map #全局参考坐标系

# Differential-drive robot configuration - necessary?
#  holonomic_robot: false