ROS激光SLAM导航理解

最新推荐文章于 2024-09-27 15:23:00 发布

teddyluo

最新推荐文章于 2024-09-27 15:23:00 发布

阅读量2.3w

点赞数 53

分类专栏： slam ROS Navigation ROS激光SLAM导航文章标签： SLAM ROS 导航同步定位与导航

slam 同时被 3 个专栏收录

20 篇文章 17 订阅

订阅专栏

ROS

14 篇文章 3 订阅

订阅专栏

ROS激光SLAM导航

14 篇文章 10 订阅

订阅专栏

ROS激光SLAM导航理解

注：最近学习ROS的激光导航知识，需要理清ROS的SLAM、环境感知(costmap)、与导航算法。为防止自己忘记，将觉得有价值的内容收集于此。对AGV来说，SLAM是个大大坑，环境感知和局部运动控制也是大坑，学习的过程就是学会怎么从坑里爬出来的过程

文章目录

ROS激光SLAM导航理解

激光SLAM基本原理

基本原理

关于机器人运动控制系统架构，在《ros by example》 chapter 7一章第二节中介绍了控制机器人的5个层次，从低到高依次是：motor controllers anddrivers-> ROS base controller ->Frame-Base Motion（move_base）->Frame-Base Motion（gmapping + amcl）->Semantic Goals。总结起来如下图所示：
在这里插入图片描述
可简单的分为三个层面，最底层，中间通信层和决策层。最底层就是机器人本身的电机驱动和控制部分，中间通信层是底层控制部分和决策层的通信通路，决策层就是负责机器人的建图定位以及导航。

本文主要研究激光SLAM(构建2D地图和导航)，所以只探讨决策层这一层的实现。我们在已有机器人最底层的前提下，采用ROS提供的Gmapping包和Navigation栈作为机器人的决策层。

1、占据栅格地图基本原理

Gmapping包是在ROS里对开源社区openslam下gmapping算法的C++实现，该算法采用一种高效的Rao-Blackwellized粒子滤波将收取到的激光测距数据最终转换为栅格地图。

机器人定位与建图通常被认为是“鸡与鸡蛋”的问题，因为这个原因才会将这个过程命名为SLAM（Simultaneous localization and mapping），所以即时定位与地图构建（SLAM）是这样一个概念：把两方面的进程都捆绑在一个循环之中，以此支持双方在各自进程中都求得连续解；不同进程中相互迭代的反馈对双方的连续解有改进作用。

占据栅格地图的构建主要采取粒子滤波的方法，粒子滤波是目前一种可以代替高斯滤波器的广为流行的滤波器是非参数化滤波器。[这句话的描述不清晰]非参数化滤波器不需要满足扩展卡尔曼滤波算法所要求的非线性滤波随机量必须满足高斯分布的条件，它也不依赖于一个固定的后验方程去估计后验状态，而是从后验概率中抽取随机状态粒子来表达其分布。粒子滤波就是一种非参数化滤波器的实现算法，粒子滤波的关键是从后验分布中产生一组随机状态样本来表示后验概率分布。

粒子滤波的思想基于蒙特卡洛方法来表示概率[粒子滤波的思想是基于蒙特卡洛方法来表示概率]，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率（观测方程）中抽取的随机状态粒子来表达其分布，是一种循序重要性采样法。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算(状态方程)，从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量 $N\rightarrow\infty$ 时可以逼近任何形式的概率分布。虽然在粒子滤波算法中，其概率分布仅仅是真实分布的一种近似，但由于粒子滤波是非参数化的，它解决了非线性滤波问题中随机量必须满足高斯分布的缺陷，能表达相较于高斯分布模型而言更为广泛的分布，也对变量参数的非线性特性有更强的建模能力。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决SLAM问题。

在粒子滤波中，后验分布的样本，我们称之为“粒子”，每一个粒子都是在时刻t的一个状态的实例化，这个实例化就是在t时刻的真实状态的假设。这里M代表粒子集中粒子的总数。在实际环境中，粒子总数M通常是一个较大的数字，在一些实现方法中，M是与时间t或者其它与状态的概率密度相关的函数。

粒子滤波的思想是通过一组粒子来估计近似状态概率。理论上，粒子集中一个假设状态粒子的概率应该与贝叶斯滤波器t时刻后验概率成一定比例关系。

粒子滤波主要步骤如下：

（1）初始化阶段：
规定粒子数量，将粒子平均的分布在规划区域，规划区域需要人为或者通过特征算法计算得出，比如人脸追踪，初始化阶段需要人为标出图片中人脸范围或者使用人脸识别算法识别出人脸区域。对于SLAM来说，规划区域一般为用来进行定位的地图，在初始化时，将需要设置的特定数量粒子均匀的撒满整张地图。

（2）转移阶段：
这个阶段所做的任务就是对每个粒子根据状态转移方程进行状态估计，每个粒子将会产生一个与之相对应的预测粒子。这一步同卡尔曼滤波方法相同，只是卡尔曼是对一个状态进行状态估计，粒子滤波是对大量样本（每个粒子即是一个样本）进行状态估计。

（3）决策阶段：
决策阶段也称校正阶段。在这一阶段中，算法需要对预测粒子进行评价，越接近于真实状态的粒子，其权重越大，反之，与真实值相差较大的粒子，其权重越小。此步骤是为重采样做准备。在SLAM中权重计算方式有很多，比如机器人行走过程中，激光雷达或者深度摄像头会返回周围位置信息，如果这些信息与期望值相差较大，亦或者在运动中某些粒子本应该没有碰到障碍或者边界，然而在运算中却到达甚至穿过了障碍点或边界，那么这种粒子就是坏点粒子，这样的粒子权重也就比较低一些。

（4）重采样阶段：
根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；权重小的粒子有些会被淘汰，为了保证粒子总数不变，一般会在权值较高的粒子附近加入一些新的粒子。

（5）滤波：
将重采样后的粒子带入状态转移方程得到新的预测粒子，然后将它们继续进行上述转移、决策、重采样过程，经过这种循环迭代，最终绝大部分粒子会聚集在与真实值最接近的区域内，从而得到机器人准确的位置，实现定位。

（6）地图生成：
每个粒子都携带一个路径地图，整个过程下来，我们选取最优的粒子，即可获得规划区域的栅格地图。

2、导航基本原理

Navigation栈[是否为Navigation包]是一个获取里程计信息、传感器数据和目标位姿并输出安全的速度命令到运动平台的2D导航包的集合。
(1) 定位
机器人在导航的过程中需要时刻确定自身当前的位置，Navigation 栈中使用amcl包来定位。amcl是一种概率定位系统，以2D方式对移动机器人定位，它实现了自适应（或者KLD-采样）蒙特卡洛定位法，使用粒子滤波跟踪机器人在已知地图中的位姿。下面的图片显示用里程计和AMCL定位的不同之处，AMCL估计base结构（机器人）相当于global结构(世界地图)TF转换（ROS中的坐标系转换）。从本质上，这种转换利用航位推算来处理漂移，所发布的转换是远期的。
在这里插入图片描述

(2)路径规划
路径导航部分则使用move_base包，move_base能够获取机器人周围信息（如激光雷达扫描结果）并生成全局与局部的代价地图，根据这些代价地图可以使机器人绕开障碍物安全到达指定的位置。move_base的路径规划主要分为全局规划和局部规划，分别采用的是A*算法和DWA（Dynamic Window Approach)）算法。

SLAM与导航系统框架: 激光SLAM系统框架

激光SLAM系统框架: 在激光SLAM系统中，Gmapping获取扫描的激光雷达信息以及里程计数据可动态的生成2D栅格地图。导航包则利用这个栅格地图，里程计数据和激光雷达数据做出适合的路径规划和定位，最后转换为机器人的速度指令。下图为激光SLAM系统的框架，方框里为传感器获得的数据或者生成的数据，椭圆里为ROS节点。

在这里插入图片描述

建图系统框架

在这里插入图片描述

（1）数据输入
在ROS GMapping包中，获取激光和里程计数据传入openslam GMapping包中，为新一时刻的建图做准备。

（2）运动模型
根据t-1时刻的粒子位姿以及里程计数据，预测t时刻的粒子位姿，在初始值的基础上增加高斯采样的noisypoint。

（3）扫描匹配
对每个粒子执行扫描匹配算法，GMapping默认采用30个采样粒子。扫描匹配的作用是找到每个粒子在t时刻位姿的最佳坐标。为后面每个粒子权重更新做准备。如果此处扫描匹配失败，则粒子权重更新则采用默认的似然估计。

（4）建议分布
混合了运动模型和观测模型的建议分布，根据上一步扫描匹配获得的最佳坐标，围绕该坐标取若干位置样本（距离差值小于某阈值）计算均值与方差，使得当前粒子位置满足该均值方差的高斯分布。

（5）权重计算
对各个粒子的权重进行更新，更新之后还需进行归一化操作。注意：重采样前更新过一次，重采样后又更新过一次。

（6）重采样
使用Neff判断是否进行重采样（重采样频率越高，粒子退化越严重，即粒子多样性降低，导致建图精确度降低，有必要设定一个判定值改善粒子退化问题）。

（7）粒子维护地图
每个粒子都维护了属于自己的地图，即运动轨迹。该步骤执行的操作是更新每个粒子维护的地图。

（8）地图更新
在ros中进行地图更新。先得到最优的粒子（使用权重和 weightSum判断），得到机器人最优轨迹，地图膨胀更新。

导航系统框架

导航系统总结来看可以分为数据收集层（传感器数据收集）、全局规划层（global_planner）、局部规划层（local_planner）、行为层（结合机器人状态和上层指令给出机器人当前行为）、控制器层（与下位机通信）。

在这里插入图片描述

机器人使用navigation栈导航时，move_base这个模块负责整个navigation 行为的调度, 包括初始化costmap与planner, 监视导航状态适时更换导航策略等[监视导航状态、适时更换导航策略等]. 涉及到行为的控制, move_base 具体实现就是有限状态机, 定义了若干recovery_behavior , 指定机器人导航过程中出问题后的行为. 具体的逻辑流程如下：

（1）move_base首先启动了global_planner和local_planner两个规划器，负责全局路径规划和局部路径规划，通过costmap组件生成自己的代价地图(global_costmap和local_costmap)。

（2）通过全局路径规划，计算出机器人到目标位置的全局路线，实现的方法是基于栅格地图的cost搜索找最优。

（3）然后通过局部规划，负责做局部避障的规划，具体navigation中实现的算法是Dynamic Windows Approach，具体流程是：

由移动底盘的运动学模型得到速度的采样空间
在采样空间中, 计算每个样本的目标函数
得到期望速度, 插值成轨迹输出
在机器人运行过程中，根据机器人的状态做出规划（唤醒规划器），操作（计算合法速度并发布），清理（recovery_behavior）等操作。

`move_base`中的使用

在move_base的使用中，首先分别定义了planner_costmap_ros_,controller_costmap_ros_分别对应gloabl_costmap, local_costmap, 在move_base的构造函数中，初始化这两个对象。这两个对象使用的是costmap_2d包中封装之后的costmap_2d::Costmap2DROS类，在这个类中包含对plugin的载入。分别如下:

    //move_base中的初始化:move_base/src/move_base.cpp
     //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
    planner_costmap_ros_->pause();
    //initialize the global planner
    try {
      planner_ = bgp_loader_.createInstance(global_planner);
      planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
      exit(1);
    }
    //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
    controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    controller_costmap_ros_->pause();
    //create a local planner
    try {
      tc_ = blp_loader_.createInstance(local_planner);
      ROS_INFO("Created local_planner %s", local_planner.c_str());
      tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
      exit(1);
    }
    // Start actively updating costmaps based on sensor data
    planner_costmap_ros_->start();
    controller_costmap_ros_->start();

在costmap2DROS中初始化对象layered_costmap_，然后向其中添加plugin:layered_costmap_->addPlugin(plugin):

// costmap_2d/src/costmap_2d_ros.cpp
  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);  
  if (!private_nh.hasParam("plugins"))   // load the default configure
  { //函数 最后一行:nh.setParam("plugins",super_array); super_array是构造的默认plugin
    //函数 起始一行:ROS_INFO("Loading from pre-hydro paramter sytle");
    //也可以看出navigation的设计，是plugin的一种方式
    resetOldParameters(private_nh);      
  }
  if (private_nh.hasParam("plugins"))    // load the plugins
  {
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str());
      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      layered_costmap_->addPlugin(plugin);
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
  }

完成每一个plugin的初始化之后，每一个地图的updateMap中先后执行updateBounds,updataCosts:

updateBounds：这个阶段会更新每个Layer的更新区域，这样在每个运行周期内减少了数据拷贝的操作时间。StaticLayer的Static map只在第一次做更新，Bounds 范围是整张Map的大小，而且在UpdateBounds过程中没有对Static Map层的数据做过任何的更新。ObstacleLayer在这个阶段主要的操作是更新Obstacles Map层的数据，然后更新Bounds。InflationLayer则保持上一次的Bounds。
updateCosts：这个阶段将各层数据逐一拷贝到Master Map，上图中展示了Master Map的产生过程。

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    ………… 
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
    {
      ROS_WARN_THROTTLE(……);
    }
  }
  …………
  ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
  if (xn < x0 || yn < y0)
    return;
  costmap_.resetMap(x0, y0, xn, yn);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
  }
  …………
  initialized_ = true;

运用`plugins`

plugins在costmap中的运用ROS中有介绍Configuring Layered Costmaps,也可以添加一些新的plugin对应新的costmap，比如navigation_layers。

plugins: 
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: obstacles,        type: "costmap_2d::VoxelLayer"}
publish_frequency: 1.0
obstacles:
    observation_sources: base_scan
    base_scan: {data_type: LaserScan, sensor_frame: /base_laser_link, clearing: true, marking: true, topic: /base_scan}

Costmap(代价地图)（上）

Costmap是机器人收集传感器信息建立和更新的二维或三维地图，可以从下图简要了解。
在这里插入图片描述
上图中，红色部分代表costmap中的障碍物，蓝色部分表示通过机器人内切圆半径膨胀出的障碍，红色多边形是footprint(机器人轮廓的垂直投影)。为了避免碰撞，footprint不应该和红色部分有交叉，机器人中心不应该与蓝色部分有交叉。

ROS的代价地图（costmap）采用网格（grid）形式，每个网格的值（cell cost）从0~255。分成三种状态：被占用（有障碍）、自由区域（无障碍）、未知区域；

具体状态和值对应有下图：
在这里插入图片描述
上图可分为五部分，其中红色多边形区域为机器人的轮廓：
（1） Lethal（致命的）:机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。
（2） Inscribed（内切）：网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。
（3） Possibly circumscribed（外切）：网格的外切圆与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
（4） Freespace（自由空间）：没有障碍物的空间。
（5） Unknown（未知）：未知的空间。

对应的cost的计算:
exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)

// costmap_2d/inflation_layer.h
  inline unsigned char computeCost(double distance) const
  {
    unsigned char cost = 0;
    if (distance == 0)
      cost = LETHAL_OBSTACLE;
    else if (distance * resolution_ <= inscribed_radius_)
      cost = INSCRIBED_INFLATED_OBSTACLE;                //254
    else
    {
      // make sure cost falls off by Euclidean distance cost 0~255
      double euclidean_distance = distance * resolution_;
      double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
      cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
    }
    return cost;
  }

ROS中costmap_2d这个包提供了一个可以配置的结构维护costmap,其中Costmap通过costmap_2d::Costmap2DROS对象利用传感器数据和静态地图中的信息来存储和更新现实世界中障碍物的信息。costmap_2d::Costmap2DROS为用户提供了纯粹的2维索引，这样可以只通过columns查询障碍物。举个例子来说，一个桌子和一双鞋子在xy平面的相同位置，有不同的Z坐标，在costm_2d::Costmap2DROS目标对应的的costmap中，具有相同的cost值。这旨在帮助规划平面空间。

Costmap由多层组成，例如在costmap_2d包中，StaticLayer（静态地图层）是第一层，ObstacleLayer（障碍物层）是第二层，InflationLayer（膨胀层）是第三层。这三层组合成了master map（最终的costmap），供给路线规划模块使用。

Costmap主接口是costmap_2d::Costmap2DROS，它维持了Costmap在ROS中大多数的相关的功能。它用所包含的costmap_2d::LayeredCostmap类来跟踪每一个层。每层使用pluginlib（ROS插件机制）来实例化并添加到LayeredCostmap类的对象中。各个层可以被独立的编译，且允许使用C++接口对costmap做出任意的改变。

costmap_2d类之间关系

costmap_2d中的类继承关系:

在这里插入图片描述

Costmap ROS接口

ROS对costmap进行了复杂的封装，提供给用户的主要接口是Costmap2DROS,而真正的地图信息是储存在各个Layer中。下图可以简要说明Costmap的各种接口的关系：
在这里插入图片描述

Costmap的ObstacleLayer和StaticLayer都继承于CostmapLayer和Costmap2D,因为它们都有自己的地图，Costmap2D为它们提供存储地图的父类，CostmapLayer为它们提供一些对地图的操作方法。而inflationLayer因为没有维护真正的地图所以只和CostmapLayer一起继承于Layer，Layer提供了操作master map的途径。

LayerdCostmap为Costmap2DROS（用户接口）提供了加载地图层的插件机制，每个插件（即地图层）都是Layer类型的。

Costmap初始化流程

在navigation的主节点move_base中，建立了两个costmap。其中planner_costmap_ros_是用于全局导航的地图，controller_costmap_ros_是用于局部导航用的地图。下图为costmap的初始化流程。
在这里插入图片描述

（1）Costmap初始化首先获得全局坐标系和机器人坐标系的转换
（2）加载各个Layer，例如StaticLayer，ObstacleLayer，InflationLayer。
（3）设置机器人的轮廓
（4）实例化了一个Costmap2DPublisher来发布可视化数据。
（5）通过一个movementCB函数不断检测机器人是否在运动
（6）开启动态参数配置服务，服务启动了更新map的线程。

Costmap更新

Costmap的更新在mapUpdateLoop线程中实现，此线程分为两个阶段：

（阶段一）UpdateBounds：这个阶段会更新每个Layer的更新区域，这样在每个运行周期内减少了数据拷贝的操作时间。StaticLayer的Static map只在第一次做更新，Bounds 范围是整张Map的大小，而且在UpdateBounds过程中没有对Static Map层的数据做过任何的更新。ObstacleLayer在这个阶段主要的操作是更新Obstacles Map层的数据，然后更新Bounds。InflationLayer则保持上一次的Bounds。
（阶段二）UpdateCosts：这个阶段将各层数据逐一拷贝到Master Map，可以通过下图观察Master Map的生成流程。（图来源于David Lu的《Layered Costmaps for Context-Sensitive Navigation》）

在（a）中，初始有三个Layer和Master costmap,Static Layer和Obstacles Layer维护它们自己的栅格地图，而inflation Layer并没有。为了更新costmap,算法首先在各层上调用自己的UpdateBounds方法（b）。为了决定新的bounds,Obstacles Layer利用新的传感器数据更新它的costmap。然后每个层轮流用UpdateCosts方法更新Master costmap的某个区域,从Static Layer开始（c），然后是Obstacles Layer(d)，最后是inflation Layer(e)。

Costmap(代价地图)（下）

costmap程序架构

在这里插入图片描述

在move_base刚启动时就建立了两个costmap，而这两个costmap都加载了三个Layer插件，它们的初始化过程如上图所示:

StaticLayer主要为处理gmapping或者amcl等产生的静态地图。
ObstacLayer主要处理机器人移动过程中产生的障碍物信息。
InflationLayer主要处理机器人导航地图上的障碍物信息膨胀（让地图上的障碍物比实际障碍物的大小更大一些），尽可能使机器人更安全的移动。

costmap在mapUpdateLoop线程中执行更新地图的操作，每个层的工作流程如下：

(1) `StaticLayer`工作流程

在这里插入图片描述

上图是StaticLayer的工作流程，updateBounds阶段将更新的界限设置为整张地图，updateCosts阶段根据rolling参数（是否采用滚动窗口）设置的值，如果是，那静态地图会随着机器人移动而移动，则首先要获取静态地图坐标系到全局坐标系的转换，再更新静态地图层到master map里。

(2)ObstacleLayer工作流程

在这里插入图片描述

上图是ObstacleLayer的工作流程，updateBounds阶段将获取传感器传来的障碍物信息经过处理后放入一个观察队列中，updateCosts阶段则将障碍物的信息更新到master map。

(3)inflationLayer工作流程

在这里插入图片描述
上图是inflationLayer的工作流程，updateBounds阶段由于本层没有维护的map，所以维持上一层地图调用的Bounds值（处理区域）。updateCosts阶段用了一个CellData结构存储master map中每个grid点的信息，其中包括这个点的二维索引和这个点附近最近的障碍物的二维索引。改变每个障碍物CELL附近前后左右四个CELL的cost值，更新到master map就完成了障碍物的膨胀。

全局路径规划

全局路径规划简介

机器人移动到目的地需要在做出具体移动策略之前先进行全局路径规划，ROS的navigation中使用global_planner包提供的一系列全局规划的算法接口（包括A*，Dijkstra）。

在本文中我们主要使用A*算法来进行全局路径规划。

A算法*

A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。公式表示为：

$f (n) = g (n) + h (n)$

其中 $f (n)$ 是从初始状态经由状态n到目标状态的代价估计， $g (n)$ 是在状态空间中从初始状态到状态n的实际代价， $h (n)$ 是从状态 $n$ 到目标状态的最佳路径的估计代价。对于路径搜索问题，状态就是图中的节点，代价就是距离。

$h (n)$ 的选取：保证找到最短路径（最优解的）条件，关键在于估价函数 $f (n)$ 的选取（或者说 $h (n)$ 的选取）。我们以 $d (n)$ 表达状态 $n$ 到目标状态的距离，那么 $h (n)$ 的选取大致有如下三种情况：

（1）如果 $h(n)\leq d(n)$ 到目标状态的实际距离，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。
（2）如果 $h (n) = d (n)$ ，即距离估计 $h (n)$ 等于最短距离，那么搜索将严格沿着最短路径进行，此时的搜索效率是最高的。
（3）如果 $h (n) > d (n)$ ,搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。

全局路径规划模块流程

在这里插入图片描述

在move_base节点中，通过类加载模块载入了BaseGlobalPlanner（全局路径规划）的实例，首先这个进行了初始化的操作，获取了一些设置参数，比如是否使用Dijkstra算法等，开启了一个叫makePlanService的服务，一旦有请求就能调用makePlan以生成全局规划路径。在makePlan中，主要分为三个步骤，也就是计算potential值，提取plan，发布plan。

全局路径规划程序架构

本节主要对全局路径规划中重要的两个模块进行程序架构的分析，分别是计算potential值和提取plan。

（1）计算potential值

将整个地图分为许多点，而每个点对应于一组起点和终点都有一个potential值，其实就是计算A*算法中的 $f (n)$ ，该模块主要步骤如下：
①将起点放入队列

queue_.push_back(Index(start_i, 0))

②将所有点的potential都设为一个极大值

std::fill(potential, potential + ns_, POT_HIGH)

③将起点的potential设为0

potential[start_i] = 0

④进入循环，继续循环的判断条件为只要队列大小大于0且循环次数小于所有格子数的2倍

while (queue_.size() > 0 && cycle < cycles)

⑤得到最小cost的索引，并删除它，如果索引指向goal(目的地)则退出算法，返回true

Index top = queue_[0];
std::pop_heap(queue_.begin(), queue_.end(), greater1());
queue_.pop_back();
int i = top.i;
if (i == goal_i)
return true;

⑥对前后左右四个点执行add函数

 add(costs, potential, potential[i], i + 1, end_x, end_y);
 add(costs, potential, potential[i], i - 1, end_x, end_y);
 add(costs, potential, potential[i], i + nx_, end_x, end_y);
 add(costs, potential, potential[i], i - nx_, end_x, end_y);

add函数中，如果是已经添加的的点则忽略，根据costmap的值如果是障碍物的点也忽略。
potential[next_i]是起点到当前点的cost即 $g (n)$ ,distance * neutral_cost_是当前点到目的点的cost即 $h (n)$ 。

potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
int x = next_i % nx_, y = next_i / nx_;
float distance = abs(end_x - x) + abs(end_y - y);

所以计算完这两个cost后，加起来即为 $f (n)$ ，将其存入队列中。

queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));

返回④继续循环。

（2）提取plan

这个模块负责使用计算好的potential值生成一条全局规划路径。该模块的主要流程如下：
①将goal（目的地）所在的点的(x,y)作为当前点加入path

int start_index = getIndex(start_x, start_y);
path.push_back(current);

②进入循环，继续循环的条件为当前点的索引不是起始点

while (getIndex(current.first, current.second) != start_index)

③搜索当前点的四周的四个临近点，选取这四个临近点的potential的值最小的点

for (int xd = -1; xd <= 1; xd++) {
  for (int yd = -1; yd <= 1; yd++) {
   if (xd == 0 && yd == 0)
     continue;
   int x = current.first + xd, y = current.second + yd;
   int index = getIndex(x, y);
   if (potential[index] < min_val) {
      min_val = potential[index];
      min_x = x;
      min_y = y;
   }
  }
}

④将potential值最小的点更改为当前点，并加入path

current.first = min_x;
current.second = min_y;
path.push_back(current);

返回②，继续循环

至此，全局规划模块就已经根据起点和终点规划处一条全局路径，再通过最后一步发布plan将其发布为ROS话题。

局部路径规划

简介

机器人在获得目的地信息后，首先经过全局路径规划规划出一条大致可行的路线，然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略，ROS中主要是使用了DWA算法。在ROS中每当move_base处于规划状态就调用DWA算法计算出一条最佳的速度指令，发送给机器人运动底盘执行。

DWA算法

DWA算法全称为dynamic window approach，其原理主要是在速度空间 $(v, w)$ 中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，再通过一个评价函数对这些轨迹打分，最优的速度被选择出来发送给下位机。

1.确定机器人模型

在动态窗口算法中，要模拟机器人的轨迹，需要知道机器人的运动模型。假设两轮移动机器人的轨迹是一段一段的圆弧或者直线（旋转速度为0时），一对（vt，wt）就代表一个圆弧轨迹,具体推导如下:
假设不是全向运动机器人，它做圆弧运动的半径为：
$\boldsymbol{r}=\boldsymbol{v} / \boldsymbol{w}$
当旋转速度w不等于0时，机器人坐标为：
$\begin{array}{l}{x=x-\frac{v}{w} \sin \left(\theta_{t}\right)+\frac{v}{w} \sin \left(\theta_{t}+w \Delta_{i}\right)} \\ {y=y-\frac{v}{w} \cos \left(\theta_{i}\right)-\frac{v}{w} \cos \left(\theta_{i}+w \Delta_{i}\right)} \\ {\theta_{i}=\theta_{i}+w \Delta_{i}}\end{array}$

2.速度采样

机器人的轨迹运动模型有了，根据速度就可以推算出轨迹。因此只需采样很多速度，推算轨迹，然后评价这些轨迹好不好就行了。

（一）移动机器人受自身最大速度最小速度的限制

$v_m=\{ v\in [v_{\min}, v_{\max}], w \in [w_{\min}, w_{\max}] \}$

（二）移动机器人受电机性能的影响:由于电机力矩有限,存在最大的加減速限制,因此移动机器人軌迹前向模拟的周期sim_period内,存在一个动态窗口,在该窗口内的速度是机器人能够实际达到的速度:

$V_d=\{ (v,w) | v\in [v_c-v_b] \triangle t, v_c + v_a\triangle t] \wedge w \in [w_c-w_b\triangle t, w_c + w_a\triangle t] \}$

（三）基于移动机器人安全的考虑:为了能够在碰到障碍物前停下来, 因此在最大减速度条件下, 速度有一个范围:
$v_a=\{ (v,w) | v\leq \sqrt{2\cdot dist(v,w)\cdot v_b} \wedge w\leq \sqrt{2\cdot dist(v,w)\cdot w_b} \}$

3.评价函数

当采样完速度后，就对每个速度所形成的每条轨迹进行评价，采用的评价函数如下：
$w)=\sigma(\alpha \cdot h e a d i n g(v, w)+\beta \cdot d i s t(v, w)+\gamma \cdot v e l o c i b(v, w))$

DWA局部路径规划程序架构

1.程序模块流程

在这里插入图片描述
（1）初始化：为DWA算法做准备，加载参数和实例化对象等
（2）采样速度样本：计算出需要评价的速度样本
（3）样本评分：对计算出的速度样本进行逐一评分，记录下评价最高的样本
（4）发布plan：发布得到的局部路径规划策略

2.DWA算法程序分析

（1）初始化：
在move_base节点中，通过类加载模块载入了BaseLocalPlanner（局部路径规划）的子类DWAPlannerROS的实例tc_，并调用其初始化函数，获取了一些初始状态信息比如机器人当前位置等，并创建了真正实现DWA算法的DWAPlanner类的实例dp_，最后设置了动态参数配置服务。dp_的构造函数做了一系列参数获取的操作，最重要的是将几种cost计算方法的实例加入一个名为critics的vector容器里。

（2）采样速度样本：
当move_base调用tc_的computeVelocityCommands方法后，tc_会调用dwaComputeVelocityCommands方法，并在其中调用dp_的findBestPath方法。findBestPath方法里调用SimpleTrajectoryGenerator类的实例generator_的initialise函数，这个函数就是主要负责速度采样的。

每个维度速度需要采样的养本数存放在vsamples_这个结构体内，vsamples_[0]是x方向样本数，vsamples_[1]是y方向样本数，vsamples_[2]是z方向样本数。首先计算各个方向的最大速度和最小速度，DWA算法只在第一步进行采样，所以

最大速度为:
Max_vel=max(max_vel,vel+acc_lim*sim_period)
最小速度为:
Min_vel=min(min_vel,vel-acc_lim*sim_period)

其中max_vel，min_vel为人为设定的最大和最小速度，vel是当前速度，acc_lim是人为设定的最大加速度，sim_period是第一步的模拟时间，由人为设定的局部路径规划频率决定，默认为0.05。

当计算出各个维度的最大最小速度后，就创建三个VelocityIterator类的对象，并传入最大最小速度和样本数目，此对象的构造函数会生成同样数目的速度样本并放入samples_这个容器内。具体做法是先计算步长step_size：

step_size=(max-min)/(nums_samples-1)

max为最大速度，min为最小速度，nums_samples为样本数目。从最小速度每次多累加一次step_size即为一个速度样本，直到达到最大速度。将每个维度的速度样本取得后，再全部循环每个样本组里选择一个组合放入结构体vel_sample，最后将这些vel_sample放入sample_params_的容器里。至此，速度采样就完成了。

（3）样本评分

速度采样完成后，逐一循环对样本空间内的样本进行评分。对每一组速度调用scoreTrajectory函数计算其评分，而scoreTrajectory函数则对这一组速度调用所有critics容器里的costfunction计算每个cost从而累加算出总的cost。在计算过程中，一旦累加的cost大于当前最小的cost则抛弃这组速度。

之前说到的几种cost成本函数为下列所示：

ObstacleCostFunction
这个成本函数基于感知障碍物来评估轨迹。它或者由于轨迹通过障碍物而返回负值，或者0。
MapGridCostFunction
这个成本函数类基于轨迹离全局路径或者接近目标点有多近来评估轨迹。这个尝试利用距离预计算地图有相同距离的路径或者目标点的所有的规划，来优惠计算速度。
在 dwa_local_planner中，代价函数因为不同的目的，被多次实例化。保持轨迹接近于路径，使机器人朝局部目标前进，并且使机器人的前段点指向局部目标。代价函数是一个启发，可以带来坏的结果或者不合适的参数的失败。
OscillationCostFunction
震荡发生在X,Y,theta维度上，正/负值被连续的选择。为了阻止震荡，当机器人在任何方向移动时，与下一个循环相反的方向被标记为无效，直到机器人已经从所设置标记的位置移动而并且超过一定的距离。这个成本函数类帮助减少某些震荡，虽然这可以有效的阻止这些震荡，如果使用不合适的参数，但是有可能阻止良好的解。
PreferForwardCostFunction
考虑到好的激光扫描范围只在机器人的前面，这个成本函数类被设计在像PR2一样的机器人上。成本函数更喜欢正面向前运动，惩罚背面运用及扫射动作。在其他机器人上或者其他领域，这可能是非常不可取的行为。

（4）发布plan
通过上述几种评分机制，选取最优的一组速度样本，传递给move_base，并发布相应的local plan。move_base如果收到了可用的速度则发布给底盘，否则发布0速度，且如果寻找最优速度的时间超过了限制就会执行障碍物清理模式，state_会变为CLEARING。

DWA 评价

在这里插入图片描述
DWA的算法流程如上图所示。通过线速度与角速度的交叉组合可以得出多组速度采样空间,依次可以通过车辆运动学模型(自行车模型)得出多组轨迹空间,由以下三组评价函数对每条轨迹进行评价:

生成轨迹与参考路径的距离(贴合程度)
生成轨迹与参考路径终点的距离
生成轨迹上是否存在障碍物(若有则抛弃这条轨迹)

优点

反应速度较快,计算不复杂,通过速度组合(线速度与角速度)可以快速得出下一时刻规划轨迹的最优解.
可以将优化由横向与纵向两个维度向一个维度优化

缺点

此算法是由机器人避障衍生而来,机器人的路径规划算法主要体现在较高的灵活性和其静态环境,譬如某些种车辆的工作环境,是一个较大的开阔广场,因此是比较适用此算法的
无人驾驶则要求较高的稳定性和动态环境,主要是结合车道线信息来实现规划算法
较高的灵活性会极大的降低行驶的平稳性
个人认为这是避障算法载体由机器人向无人车平台移植过程中会产生的主要问题

Timed-Elastic-Band算法

注：

teb局部路径规划算法github地址：https://github.com/rst-tu-dortmund/teb_local_planner。
作者列出的几篇文章均推荐阅读了解。teb的文章为《Trajectory modification considering dynamic constraints of autonomous robots》一文。
关于time eletic band这个概念，在这推荐看一下qqfly的文章，非常生动形象：link

qqfly文章中:

关于eletic band（橡皮筋）的定义：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。
关于time eletic band的简述：起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time，即为Timed-Elastic-Band算法。

《Trajectory modification considering dynamic constraints of autonomous robots》的详细解读可参考《Timed-Elastic-Band局部路径规划算法》，文章地址为：
https://blog.csdn.net/xiekaikaibing/article/details/83417223