服务型移动机器人如何实现室内路径全覆盖清扫给你一个清爽干净的家

本文链接：https://blog.csdn.net/ZhangRelay/article/details/109249378

0.本文初心：

一直有朋友在我博文中关于扫地机器人（侧重区域覆盖算法）和物流机器人（侧重运输调度算法）留言交流，受限于时间一直没有完整回复，这段时间稍稍有空，加班由7127变成了997，终于可以写一篇了。

1.预备知识：

如何让忙碌了一天的程序员到家后发现地面一尘不染，做一个扫地机器人吧。很难吗？当然不难，超简单的，不信？

服务型移动机器人如何实现室内路径全覆盖清扫给你一个清爽干净的家（调试完整版记录）

1.1机器人模型

扫地机器人主要有两种模型哦，一种两个轮子适合普通家用，还有一种四个轮子适合体育馆超市等大型空间使用。

这里以运动学模型介绍为主，动力学有点难……

请务必注意！！！这种机器人不能横着走，不能横着走，不能横着走！！！

我们都知道两轮的扫地机器人可以前进，后退，左转和右转，但是不能侧向平移，为啥，如何更专业的描述这一特性，其数学模型给出了非常明确的答案！无论左轮和右轮如何旋转， $\nu _{y}$ 都是等于0的啊！

机器人建立准确的数学模型，对于理解，分析和控制此系统有着非常重要的作用。

这类机器人的运动学模型有两类，分别如下所示：

这是依据左右轮速度建立的模型，我们通常习惯用线速度和角速度去分析机器人：

设计时候使用II型，控制机器人用I型，很简单吧，但是这还不够啊，还需要知道我们的环境地图，看下节->

1.2环境地图

到了陌生的地方自然离不开地图，回到熟悉的地方，我们有记忆中的地图，可见地图对于定位，导航，路径，任务规划等必不可少，非常重要。常见的平面地图有如下：

大环境：

小环境：

那么我们扫地机器人需要啥地图呢？？？如何建立这些地图呢？？？分别由仿真和实物两种方式，这里介绍仿真，实物放在文末。

一个典型室内环境如“初心”中的案例所示，这里再举一个图书馆的案例：

对这个环境用机器人SLAM建立的环境地图如下，详细内容参考对应详细介绍的博文：

有了这个地图，就可以实现机器人在环境下的各种路径规划类相关任务设计啦。

啥是路径规划？？？

这张图是怎么来的？？？

1.3路径规划算法

覆盖

简单说，覆盖就是导航点覆盖了地图区域范围内所有机器人可以运动到的点，并且机器人必须全部走一遍！

多么痛苦的到此一游啊！

具体参考2016年博文：扫地机器人算法的一些想法和测试

2.技术解析：

扫地机器人核心是区域覆盖算法要和机器人模型相匹配，例如：

设置了不同机器人自身半径和清扫半径的区域覆盖图如下：

这时候发现为了防止出现卡住等极端情况，门狭小的房间并未在清扫规划范围呢。

清扫非常彻底，但是路径规划很密集。

对于环境简单或复杂的地图，如果同一个算法都能适用，那说明算法的适应性非常好！！！

简单地图：

所有算法测试都需要经过从简单到复杂的过程，不要急于求成啊。

复杂地图：

这里选用“初心”中的环境构建出的地图：

具体的清扫效果如何呢？看看，为了区别色彩调了一下：

啥，扫地速度太慢了？？？这样机器人工作只能724，没关系，机器人可以24小时工作的，只要“电”管够！

如果想快一点？参考这篇博文：https://zhangrelay.blog.csdn.net/article/details/76850690

三个机器人一起清扫如何？

如果还不满意，要用真实机器人自己调试扫地机器人路径规划算法？看看这一篇，也许能帮到你：

tianbot_mini机器人上手ROS/SLAM/Navigation究竟有多简单？？？

3.参考文献

Full Coverage Path Planner (FCPP)

4.核心代码

full_coverage_path_planner.cpp

//
// Copyright [2020] Nobleo Technology"  [legal/copyright]
//
#include <list>
#include <vector>

#include "full_coverage_path_planner/full_coverage_path_planner.h"

/*  *** Note the coordinate system ***
 *  grid[][] is a 2D-vector:
 *  where ix is column-index and x-coordinate in map,
 *  iy is row-index and y-coordinate in map.
 *
 *            Cols  [ix]
 *        _______________________
 *       |__|__|__|__|__|__|__|__|
 *       |__|__|__|__|__|__|__|__|
 * Rows  |__|__|__|__|__|__|__|__|
 * [iy]  |__|__|__|__|__|__|__|__|
 *       |__|__|__|__|__|__|__|__|
 *y-axis |__|__|__|__|__|__|__|__|
 *   ^   |__|__|__|__|__|__|__|__|
 *   ^   |__|__|__|__|__|__|__|__|
 *   |   |__|__|__|__|__|__|__|__|
 *   |   |__|__|__|__|__|__|__|__|
 *
 *   O   --->> x-axis
 */

// #define DEBUG_PLOT

// Default Constructor
namespace full_coverage_path_planner
{
FullCoveragePathPlanner::FullCoveragePathPlanner() : initialized_(false)
{
}

void FullCoveragePathPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path)
{
  if (!initialized_)
  {
    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
    return;
  }

  // create a message for the plan
  nav_msgs::Path gui_path;
  gui_path.poses.resize(path.size());

  if (!path.empty())
  {
    gui_path.header.frame_id = path[0].header.frame_id;
    gui_path.header.stamp = path[0].header.stamp;
  }

  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
  for (unsigned int i = 0; i < path.size(); i++)
  {
    gui_path.poses[i] = path[i];
  }

  plan_pub_.publish(gui_path);
}

void FullCoveragePathPlanner::parsePointlist2Plan(const geometry_msgs::PoseStamped& start,
    std::list<Point_t> const& goalpoints,
    std::vector<geometry_msgs::PoseStamped>& plan)
{
  geometry_msgs::PoseStamped new_goal;
  std::list<Point_t>::const_iterator it, it_next, it_prev;
  int dx_now, dy_now, dx_next, dy_next, move_dir_now = 0, move_dir_prev = 0, move_dir_next = 0;
  bool do_publish = false;
  float orientation = eDirNone;
  ROS_INFO("Received goalpoints with length: %lu", goalpoints.size());
  if (goalpoints.size() > 1)
  {
    for (it = goalpoints.begin(); it != goalpoints.end(); ++it)
    {
      it_next = it;
      it_next++;
      it_prev = it;
      it_prev--;

      // Check for the direction of movement
      if (it == goalpoints.begin())
      {
        dx_now = it_next->x - it->x;
        dy_now = it_next->y - it->y;
      }
      else
      {
        dx_now = it->x - it_prev->x;
        dy_now = it->y - it_prev->y;
        dx_next = it_next->x - it->x;
        dy_next = it_next->y - it->y;
      }

      // Calculate direction enum: dx + dy*2 will give a unique number for each of the four possible directions because
      // of their signs:
      //  1 +  0*2 =  1
      //  0 +  1*2 =  2
      // -1 +  0*2 = -1
      //  0 + -1*2 = -2
      move_dir_now = dx_now + dy_now * 2;
      move_dir_next = dx_next + dy_next * 2;

      // Check if this points needs to be published (i.e. a change of direction or first or last point in list)
      do_publish = move_dir_next != move_dir_now || it == goalpoints.begin() ||
                   (it != goalpoints.end() && it == --goalpoints.end());
      move_dir_prev = move_dir_now;

      // Add to vector if required
      if (do_publish)
      {
        new_goal.header.frame_id = "map";
        new_goal.pose.position.x = (it->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5;
        new_goal.pose.position.y = (it->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5;
        // Calculate desired orientation to be in line with movement direction
        switch (move_dir_now)
        {
        case eDirNone:
          // Keep orientation
          break;
        case eDirRight:
          orientation = 0;
          break;
        case eDirUp:
          orientation = M_PI / 2;
          break;
        case eDirLeft:
          orientation = M_PI;
          break;
        case eDirDown:
          orientation = M_PI * 1.5;
          break;
        }
        new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(orientation);
        if (it != goalpoints.begin())
        {
          previous_goal_.pose.orientation = new_goal.pose.orientation;
          // republish previous goal but with new orientation to indicate change of direction
          // useful when the plan is strictly followed with base_link
          plan.push_back(previous_goal_);
        }
        ROS_DEBUG("Voila new point: x=%f, y=%f, o=%f,%f,%f,%f", new_goal.pose.position.x, new_goal.pose.position.y,
                  new_goal.pose.orientation.x, new_goal.pose.orientation.y, new_goal.pose.orientation.z,
                  new_goal.pose.orientation.w);
        plan.push_back(new_goal);
        previous_goal_ = new_goal;
      }
    }
  }
  else
  {
    new_goal.header.frame_id = "map";
    new_goal.pose.position.x = (goalpoints.begin()->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5;
    new_goal.pose.position.y = (goalpoints.begin()->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5;
    new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(0);
    plan.push_back(new_goal);
  }
  /* Add poses from current position to start of plan */

  // Compute angle between current pose and first plan point
  double dy = plan.begin()->pose.position.y - start.pose.position.y;
  double dx = plan.begin()->pose.position.x - start.pose.position.x;
  // Arbitrary choice of 100.0*FLT_EPSILON to determine minimum angle precision of 1%
  if (!(fabs(dy) < 100.0 * FLT_EPSILON && fabs(dx) < 100.0 * FLT_EPSILON))
  {
    // Add extra translation waypoint
    double yaw = std::atan2(dy, dx);
    geometry_msgs::Quaternion quat_temp = tf::createQuaternionMsgFromYaw(yaw);
    geometry_msgs::PoseStamped extra_pose;
    extra_pose = *plan.begin();
    extra_pose.pose.orientation = quat_temp;
    plan.insert(plan.begin(), extra_pose);
    extra_pose = start;
    extra_pose.pose.orientation = quat_temp;
    plan.insert(plan.begin(), extra_pose);
  }

  // Insert current pose
  plan.insert(plan.begin(), start);

  ROS_INFO("Plan ready containing %lu goals!", plan.size());
}

bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_,
                                        std::vector<std::vector<bool> >& grid,
                                        float robotRadius,
                                        float toolRadius,
                                        geometry_msgs::PoseStamped const& realStart,
                                        Point_t& scaledStart)
{
  int ix, iy, nodeRow, nodeColl;
  uint32_t nodeSize = dmax(floor(toolRadius / cpp_grid_.info.resolution), 1);  // Size of node in pixels/units
  uint32_t robotNodeSize = dmax(floor(robotRadius / cpp_grid_.info.resolution), 1);  // RobotRadius in pixels/units
  uint32_t nRows = cpp_grid_.info.height, nCols = cpp_grid_.info.width;
  ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize);

  if (nRows == 0 || nCols == 0)
  {
    return false;
  }

  // Save map origin and scaling
  tile_size_ = nodeSize * cpp_grid_.info.resolution;  // Size of a tile in meters
  grid_origin_.x = cpp_grid_.info.origin.position.x;  // x-origin in meters
  grid_origin_.y = cpp_grid_.info.origin.position.y;  // y-origin in meters

  // Scale starting point
  scaledStart.x = static_cast<unsigned int>(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0,
                             floor(cpp_grid_.info.width / tile_size_)));
  scaledStart.y = static_cast<unsigned int>(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0,
                             floor(cpp_grid_.info.height / tile_size_)));

  // Scale grid
  for (iy = 0; iy < nRows; iy = iy + nodeSize)
  {
    std::vector<bool> gridRow;
    for (ix = 0; ix < nCols; ix = ix + nodeSize)
    {
      bool nodeOccupied = false;
      for (nodeRow = 0; (nodeRow < robotNodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow)
      {
        for (nodeColl = 0; (nodeColl < robotNodeSize) && ((ix + nodeColl) < nCols); ++nodeColl)
        {
          int index_grid = dmax((iy + nodeRow - ceil(static_cast<float>(robotNodeSize - nodeSize) / 2.0))
                            * nCols + (ix + nodeColl - ceil(static_cast<float>(robotNodeSize - nodeSize) / 2.0)), 0);
          if (cpp_grid_.data[index_grid] > 65)
          {
            nodeOccupied = true;
            break;
          }
        }
      }
      gridRow.push_back(nodeOccupied);
    }
    grid.push_back(gridRow);
  }
  return true;
}
}  // namespace full_coverage_path_planner