机器人局部避障的动态窗口法(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)空间中采样多组速度，并模拟机器人在这些速度下一定时间内的轨迹。
在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。
动态窗口的含义是依据移动机器人的加减速性能限定速度采样空间在一个可执行的范围内。

ros navigation stack 各个包的作用

nav_core

该包定义了整个导航系统关键包的接口函数，包括base_global_planner, base_local_planner以及recovery_behavior的接口。
里面的函数全是虚函数，所以该包只是起到规范接口的作用，真正功能的
实现在相应的包当中。

global_planner和navfn

这两个包干的事情是一样的，都是为实现目标点与当前点之间的全局路径规划，内部都有Dijkstra算法和A*导航算法的实现，
ROS系统默认采用的是navfn。

base_local_planner

完成局部窗口内的路径规划任务，机器人行动速度的具体生成在此包当中完成。目前有两种局部路径规划算法实现，
一是航迹推算法（TrajectoryROS），一是动态窗口法(DWA)，
该包内部的默认实现是航迹推算法，但是留出了DWA的定义接口，DWA的实现在dwa_local_planner中。

map_server

主要功能是读取pgm和yaml配套的地图文件，并将之转换到/map话题发送出来（比如rviz中显示的地图就是订阅了该话题），
另外还提供地图保存等增值服务。

costmap_2d

以层的概念来组织图层，用户可以根据需要自己配置，默认的层有static_layer（通过订阅map_server的/map主题）来生成，
obstacle_layer根据传感器获得的反馈来生成障碍物图层， inflation_layer则是将前两个图层的信息综合进行缓冲区扩展。
该包可进行动态窗口的生成（比如base_local_planner中使用的就是动态的局部窗口地图）也可以生成静态地图
（比如global_planner中使用的就是全局静态地图）。（有待验证的疑问，是否静态地图中obstacle_layer不起作用，而动态地图中static_layer不起作用）。
此外，该包还在实时发送自己的地图信息，通过nav_msgs::OccupancyGrid发送消息给rviz。所以可以仿照该写法来发送自己想要的数据到rviz中去。

costmap_2d这个包提供了一种2D代价地图的实现方案，该方案利用输入的传感器数据，构建数据2D或者3D代价地图（取决于是否使用基于voxel的实现），并根据占用网格和用户定义的膨胀半径计算2D代价地图的代价。 此外，该包也支持利用map_server初始化代价地图，支持滚动窗口的代价地图，支持参数化订阅和传感器主题的配置。

注：红色代表代价地图中的障碍物，蓝色代表的是通过机器人内切圆半径计算的障碍物膨胀，红色的多边形代表机器人外壳。为了使机器人不碰到障碍物，机器人的外壳绝对不允许与红色单元相交，机器人的中心绝对不允许与蓝色单元相交。

costmap_2d包提供了一个可配置的结构，以网格占有的形式保存机器人在该处网格的导航信息。该包使用静态地图的传感器数据与信息，通过 costmap_2d::Costmap2DROS 来储存和升级世界中的障碍物信息。The costmap_2d::Costmap2DROS对象为用户提供了一个纯粹的二维界面，这意味着对障碍物的查询只能在列中进行，例如，在XY平面上位于同一位置的桌子和鞋，虽然在Z方向上有差异但是它们在costmap_2d::Costmap2DROS对象代价地图中对应的cell上拥有相同的代价值。 这种设计对平面空间进行路径规划是有帮助的。

从Hydro发布版本开始， 用来写数据到代价地图的底层方法已经完全可配置了。 每种功能放置一层中。 例如，静态地图是一层，障碍物是另一层。 缺省情况下，障碍物层维护的是3D信息，3D障碍物数据可以让层更加灵活的标记和清除障碍物。该包提供的ROS化功能接口主要就是costmap_2d::Costmap2DROS，它使用costmap_2d::LayeredCostmap 来跟踪每一层。 每一层在Costmap2DROS中以插件方式被实例化，并被添加到LayeredCostmap。 每一层可以独立编译，且可使用C++接口实现对代价地图的随意修改。costmap_2d::Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构。关于代价地图如何更新占用珊格的细节将在下面介绍，同时提供了页面链接供查看某一层具体如何工作。

2、障碍物清楚与占有（clearing and mark）

代价地图自动订阅传感器发布的主题并基于数据进行相应自我更新。 对每个传感器来说，其可以用来执行mark（将障碍物信息插入到代价地图），也可以用来执行clear（从代价地图移除障碍物）或者二者都执行。marking操作就是索引到数组内修改cell的代价。然而对于clearing操作，每次观测报告都需要传感器源向被观测对象发射线。 如果存储的障碍物信息是3D的，需要将每一列的障碍物信息投影成2D后才能放入到代价地图。
3、空间状态（占有、*、未知）

虽然代价地图中每个cell可用255个不同值中任何一个值(see the inflation section)，可是下层数据结构仅需要3个值。 具体来说在这种下层结构中，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。 当投影到代价地图时候，每种状态被赋一个特定的代价值。 如果列有一定量的占用(see mark_threshold parameter) 就被赋代价值costmap_2d::LETHAL_OBSTACLE， 如果列有一定量的unknown cells (see unknown_threshold parameter) 就被赋代价值costmap_2d::NO_INFORMATION, 剩余其它列赋代价值为costmap_2d::FREE_SPACE。

4、地图更新

代价地图以参数update_frequency 指定的周期进行地图更新。每个周期传感器数据进来后，都要在代价地图底层占用结构上执行标记和清除障碍操作，并且这种结构会被投影到代价地图附上相应代价值。 这完成之后，对代价赋值为costmap_2d::LETHAL_OBSTACLE的每个cell执行障碍物的膨胀操作，即从每个占用cell向外传播代价值，直到用户定义的膨胀半径为止。细节如下6.
5、tf
为了把来自传感器源的数据插入到代价地图，costmap_2d::Costmap2DROS要大量使用tf。所有tf转换由global_frame parameter，the robot_base_frame parameter，以及sensor sources建立联系和更新。The transform_tolerance parameter定义了tf转换所能容忍的的最大延时。如果 tf 树没有以期望速度被更新，那么导航功能包集将会让机器人停止。
6、膨胀（Inflation）

Inflation is the process of propagating cost values out from occupied cells that decrease with distance. For this purpose, we define 5 specific symbols for costmap values as they relate to a robot.

膨胀是由被占有的栅格（occupied cells ）向外传播其代价值的过程，传播的值随距离增加而递减。为了实现该目的，定义了五个与机器人相关联的特殊符号。

具体状态和值对应有下图：

“Lethal” cost：意思是cell内有障碍物，因此如果机器人中心位于那个cell，很显然机器人会碰到障碍
“Inscribed” cost：意思是cell到障碍物的距离小于机器人内切圆半径。 因此如果机器人中心在cell（或者代价>=内切代价），机器人与障碍物肯定有冲突
“Possibly circumscribed” cost：类似于内切，但是使用机器人外接圆作为截止距离。因此，如果机器人中心位于cell上（或者代价>=外接代价），那么它与障碍物是否冲突依赖于机器人方位。使用术语“可能”意思是这不一定是一个真正障碍cell，但是有些用户喜欢放特定的代价值进去为特定目的。 例如，如果用户要表示机器人应该尝试避开建筑物某特定区域， 它们可以在代价地图上为那个与任何障碍毫无关系的区域（该区域本来就不是障碍区）插入代价值。注意，虽然上图示例中使用了代价值128，但是实际由于代价值受内切圆半径和外接圆半径的影响，真正实际值并不一定是128。

“Freespace” cost： 假定为0，意思是没啥东西会阻碍机器人到那里
“Unknown” cost：意思是cell上没有信息

All other costs： 被赋值为"Freespace"和"Possibly circumscribed"之间的一个值，取决于到 “Lethal” cell的距离以及用户定义的衰减函数

costmap简单来说就是为了在这张地图上进行各种加工，方便我们后面进行路径规划而存在的。那具体该如何实现costmap呢？在ROS中使用costmap_2d这个软件包来实现的，该软件包在原始地图上实现了两张新的地图。一个是local_costmap，另外一个就是global_costmap，根据名字大家就可以知道了，两张costmap一个是为局部路径规划准备的，一个是为全局路径规划准备的。无论是local_costmap还是global_costmap，都可以配置多个图层，包括下面几种：

Static Map Layer：静态地图层，基本上不变的地图层，通常都是SLAM建立完成的静态地图。
Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。
Other Layers：你还可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。

rotate_recovery和clear_costmap_recovery

这两个包都继承自nav_core中定义的recovery_behavior类， 具体实现的是当导航发现无路可走的时候，
机器人在原地打转转并清理（更新更恰当些）周围障碍物信息看是否有动态障碍物运动开，
能找到路继续走。在move_base当中，默认使用的recovery_behavior是这样的，先进行一次旋转，然后进行一次小半径的障碍物更新，然后再进行一次旋转，再进行一次大范围的障碍物更新，每进行一次recovery_behavior，都会重新尝试进行一次局部寻路，如果没找到，
才会再执行下一个recovery_behavior。这两个包的代码非常简单，不看也不会影响理解偏差，
所以可以不用浪费时间看，当然因为简单要看也是分分钟的事。

move_base

这个是整个navigation stack当中进行宏观调控的看得见的手。
它主要干的事情是这样的： 维护一张全局地图（基本上是不会更新的，一般是静态costmap类型），维护一张局部地图（实时更新，costmap类型），
维护一个全局路径规划器global_planner完成全局路径规划的任务， 维护一个局部路径规划器base_local_planner完成局部路径规划的任务。
然后提供一个对外的服务，负责监听nav_msgs::goal类型的消息，然后调动全局规划器规划全局路径，再将全局路径送进局部规划器，
局部规划器结合周围障碍信息（从其维护的costmap中查询），全局路径信息，目标点信息采样速度并评分获得最高得分的轨迹（即是采样的最佳速度），
然后返回速度值，由move_base发送Twist类型的cmd_vel消息上，从而控制机器人移动。完成导航任务。

在启动move_base节点时，可以看到我们首先加载了costmap_common_params.yaml到global_costmap和local_costmap两个命名空间中，因为该配置文件是一个通用的代价地图配置参数，即local_costmap和global_costmap都需要配置的参数。然后下面是local_costmap_params.yaml专门为了局部代价地图配置的参数，global_costmap_params.yaml专门为全局代价地图配置的参数。

配置costmap_common_params.yaml

FileName: costmap_common_params.yaml
# 代价地图通用参数配置文件,就是全局代价地图和局部代价地图
# 共同都需要配置的参数,各参数意义如下:
# robot_radius: 机器人的半径
robot_radius: 0.2
obstacle_layer:
  enabled: true
  combination_method: 1
  track_unknown_space: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: laser_scan_sensor
  laser_scan_sensor: {
    sensor_frame: /robot0_laser_0,
    data_type: LaserScan,
    topic: /robot0/laser_0,
    marking: true,
    clearing: true
  }
inflation_layer:
  enabled: true
  cost_scaling_factor: 5.0
  inflation_radius: 0.36
static_layer:
  enabled: true

下面来依次解释下各参数的意义，方便大家以后来根据需要来自行修改调试：

robot_radius：设置机器人的半径，单位是米。由于在stdr中机器人是圆形的，所以可以直接设置该参数。如果你的机器人不是圆形的那就需要使用footprint这个参数，该参数是一个列表，其中的每一个坐标代表机器人上的一点，设置机器人的中心为[0,0]，根据机器人不同的形状，找到机器人各凸出的坐标点即可，具体可参考下图来设置：

footprint.png

obstacle_layer:配置障碍物图层

enabled:是否启用该层

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

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

obstacle_range:设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物;

raytrace_range:在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可*移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以*移动的空间。

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

laser_scan_sensor:添加的激光雷达传感器

sensor_frame:激光雷达传感器的坐标系名称;

data_type:激光雷达数据类型;

topic:该激光雷达发布的话题名;

marking:是否可以使用该传感器来标记障碍物;

clearing:是否可以使用该传感器来清除障碍物标记为*空间;

inflation_layer:膨胀层，用于在障碍物外标记一层危险区域，在路径规划时需要避开该危险区域

enabled:是否启用该层;

cost_scaling_factor:膨胀过程中应用到代价值的比例因子，代价地图中到实际障碍物距离在内切圆半径到膨胀半径之间的所有cell可以使用如下公式来计算膨胀代价：exp(-1.0 * cost_scaling_factor * (distance_from_obstacle – inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE – 1),公式中costmap_2d::INSCRIBED_INFLATED_OBSTACLE目前指定为254，注意： 由于在公式中cost_scaling_factor被乘了一个负数，所以增大比例因子反而会降低代价

inflation_radius:膨胀半径，膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。



Static_layer:静态地图层，即SLAM中构建的地图层

enabled:是否启用该地图层;

global_costmap_params.yaml
全局代价地图是作为进行全局路径规划时的参考，我们需要在config目录中，创建global_costmap_params.yaml文件，该文件是为全局代价地图配置的参数，具体配置的参数如下：
FileName: global_costmap_params.yaml
Description:
全局代价地图参数配置文件,各参数的意义如下：
global_frame:在全局代价地图中的全局坐标系;
robot_base_frame:机器人的基坐标系;
global_costmap:
global_frame: /map
robot_base_frame: /robot0
update_frequency: 0.5
static_map: true
rolling_window: false
transform_tolerance: 1.0
plugins:
- {name: static_layer, type: “costmap_2d::StaticLayer”}
- {name: obstacle_layer, type: “costmap_2d::ObstacleLayer”}
- {name: inflation_layer, type: “costmap_2d::InflationLayer”}

下面我们来详细解释下该全局代价地图配置文件中各参数的意义：
global_frame:全局代价地图需要在哪个坐标系下运行;
robot_base_frame:在全局代价地图中机器人本体的基坐标系，就是机器人上的根坐标系。通过global_frame和robot_base_frame就可以计算两个坐标系之间的变换，得知机器人在全局坐标系中的坐标了。
update_frequency:全局代价地图更新频率，一般全局代价地图更新频率设置的比较小;
static_map:配置是否使用map_server提供的地图来初始化，一般全局地图都是静态的，需要设置为true;
rolling_window:是否在机器人移动过程中需要滚动窗口，始终保持机器人在当前窗口中心位置;
transform_tolerance:坐标系间的转换可以忍受的最大延时;
plugins:在global_costmap中使用下面三个插件来融合三个不同图层，分别是static_layer、obstacle_layer和inflation_layer，合成一个master_layer来进行全局路径规划。

local_costmap_params.yaml

局部代价地图配置参数所建立的地图主要是为局部路径规划所使用，我们可以在config目录下，创建local_costmap_params.yaml文件，完整内容如下：

FileName: local_costmap_params.yaml
Description:
本地代价地图需要配置的参数，各参数意义如下:
global_frame:在本地代价地图中的全局坐标系;
robot_base_frame:机器人本体的基坐标系;
local_costmap:
  global_frame: /map_static
  robot_base_frame: /robot0
  update_frequency: 5.0
  publish_frequency: 3.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.05
  transform_tolerance: 0.5
  plugins:
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

下面来详细解释下每个参数的意义：
global_frame:在局部代价地图中的全局坐标系，一般需要设置为odom_frame，但是由于stdr没有这个坐标系，我就拿/map_static来代替了;
robot_base_frame:机器人本体的基坐标系;
update_frequency:局部代价地图的更新频率;
publish_frequency:局部代价地图的发布频率;
static_map:局部代价地图一般不设置为静态地图，因为需要检测是否在机器人附近有新增的动态障碍物;
rolling_window:使用滚动窗口，始终保持机器人在当前局部地图的中心位置;
width:滚动窗口的宽度，单位是米;
height:滚动窗口的高度，单位是米;
resolution:地图的分辨率，该分辨率可以从加载的地图相对应的配置文件中获取到;
transform_tolerance:局部代价地图中的坐标系之间转换的最大可忍受延时;
plugins:在局部代价地图中，不需要静态地图层，因为我们使用滚动窗口来不断的扫描障碍物，所以就需要融合两层地图（inflation_layer和obstacle_layer）即可，融合后的地图用于进行局部路径规划;