文献阅读之Voronoi图的生成与更新

最新推荐文章于 2025-04-14 10:45:12 发布

原创

最新推荐文章于 2025-04-14 10:45:12 发布 · 7.1k 阅读

标签

#导航 #路径规划 #避障 #Voronoi #动态更新

本文详细解析了机器人导航中Voronoi图的生成与更新算法，重点介绍了DistanceMap(DM)的更新机制，包括障碍物添加、移除、更新的处理流程，以及Voronoi图数据结构的设计。通过分析BorisLau的论文和代码，阐述了Voronoi图在机器人路径规划和避障中的应用。

大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

通俗的说，在机器人导航方面，Voronoi图是一种将地图分区的方法，分区的界限即Voronoi图的边，常用作机器人远离障碍物避障的规划路径。本文主要参考了 Boris Lau 的论文和代码，对Voronoi图的生成和更新进行分析。相关的3篇论文内容重合度比较高，我主要以《Efficient Grid-Based Spatial Representations for Robot Navigation in Dynamic Environments》为主。对代码的理解和注释，我已在GitHub上开源，欢迎大家一起讨论。

1. DM的更新概述

在机器人路径规划和避障的过程中，我们常常需要知道某个时刻机器人与最近障碍物的距离，以远离障碍物，或者进行碰撞检测。论文提出使用Distance Map（DM）和 Generalized Voronoi Diagrams（GVD）来解决这个问题。DM的建立和更新是GVD建立和更新的前提，方法来源于改进的brushfire算法，过程如图1-2所示。DM的每个栅格都会保存与最近障碍物点的距离，以及障碍物点的坐标（因此，障碍物的内部点是被忽略的，只有轮廓点被考察）。

图1A是论文算法的输入——已知的二值占据栅格地图，其中外围的黑色是地图外部区域，中间的黑色是障碍物，白色是可行驶区域。因为有边界和障碍物的存在，使得内部白色栅格与最近障碍物点的距离会减小（初始化为正无穷），因此要从障碍物栅格开始，逐步向外扩散更新，计算新的最近障碍物坐标与距离，反映为图1B-D中灰色逐步扩展，距离越近颜色越深。当所有栅格都被更新后，DM建立完成。

当图1中的障碍物消失、新的障碍物出现时（图2B），相应的二值占据栅格地图会被更新，进而触发DM和GVD的更新。因为旧的障碍物（记为P）消失，那么周围以P为最近障碍物的栅格，暂时没有最近障碍物，其保存的最近障碍物距离也会被置为无效值（或正无穷，或初始值），所以这些栅格的状态更新是一个距离增大（raise）的过程。

类似的，因为新的障碍物（记为Q）出现，那么Q周围的栅格，其保存的最近障碍物距离被重新计算（可能是到Q的距离），所以这些栅格的状态更新是一个距离减小（lower）的过程。

当raise和lower的过程相遇，lower处理过的栅格不会受影响，但是raise处理过的栅格，这时就要考虑新出现的Q对其的影响，就要重新计算最近障碍物（可能是Q）的距离，所以raise过程结束，转变为lower过程（图2C）。

当raise和lower都不再进展，DM更新结束。在DM更新的过程中，GVD会同步更新，我会在接下来的代码中展示GVD的更新过程。障碍物的移动，也可以分解为原位置的障碍物消失、新位置的障碍物出现的过程。因为更新不会遍历所有的栅格（比如最外层的栅格，其最近的障碍物一定是地图边界，无需更新也不会更新），所以这是一个增量更新的过程，访问栅格少，实时性好。

2. Voronoi数据结构

// queues
//保存待考察的栅格
BucketPrioQueue<INTPOINT> open_;
//保存待剪枝的栅格
std::queue<INTPOINT> pruneQueue_;
//保存预处理后的待剪枝的栅格
BucketPrioQueue<INTPOINT> sortedPruneQueue_;

//保存移除的障碍物曾占据的栅格
std::vector<INTPOINT> removeList_;
//保存增加的障碍物要占据的栅格
std::vector<INTPOINT> addList_;
//保存上次添加的障碍物覆盖的栅格
std::vector<INTPOINT> lastObstacles_;

// maps
int sizeY_;
int sizeX_;
dataCell** data_;         //保存了每个栅格与最近障碍物的距离、最近障碍物的坐标、是否Voronoi点的标志
bool** gridMap_;          //true是被占用，false是没有被占用
bool allocatedGridMap_;   //是否为gridmap分配了内存的标志位

DM和GVD的栅格用dataCell二维数组表示，gridMap_是输入的二值占据栅格地图。

struct dataCell {
    float dist;
    char voronoi;   //State的枚举值
    char queueing;  //QueueingState的枚举值
    int obstX;
    int obstY;
    bool needsRaise;
    int sqdist;
};

使用到的枚举型状态量如下，最终state是voronoiKeep 的点，便是Voronoi的边上的点，组成了Voronoi图。QueueingState 的含义我没有搞明白，但是不妨碍理解算法的思路和流程。

typedef enum {voronoiKeep=-4, freeQueued = -3, voronoiRetry=-2, voronoiPrune=-1, free=0, occupied=1} State;
//下面这几个枚举状态没搞懂
typedef enum {fwNotQueued=1, fwQueued=2, fwProcessed=3, bwQueued=4, bwProcessed=1} QueueingState;
typedef enum {invalidObstData = SHRT_MAX/2} ObstDataState;
//表示剪枝操作时栅格的临时状态
typedef enum {pruned, keep, retry} markerMatchResult;

3. 地图数据初始化

//输入二值地图gridmap，根据元素是否被占用，更新data_
void DynamicVoronoi::initializeMap(int _sizeX, int _sizeY, bool** _gridMap) {
  gridMap_ = _gridMap;
  initializeEmpty(_sizeX, _sizeY, false);

  for (int x=0; x<sizeX_; x++) {
    for (int y=0; y<sizeY_; y++) {
      if (gridMap_[x][y]) {             //如果gridmap_中的(x,y)被占用了
        dataCell c = data_[x][y];
        if (!isOccupied(x,y,c)) {       //如果c没有被占用，即data_中的(x,y)没被占用，需要更新
          bool isSurrounded = true;     //如果在gridmap_中的邻居元素全被占用
          for (int dx=-1; dx<=1; dx++) {
            int nx = x+dx;
            if (nx<=0 || nx>=sizeX_-1) continue;
            for (int dy=-1; dy<=1; dy++) {
              if (dx==0 && dy==0) continue;
              int ny = y+dy;
              if (ny<=0 || ny>=sizeY_-1) continue;

              if (!gridMap_[nx][ny]) {  //如果在gridmap_中的邻居元素有任意一个没被占用（就是障碍物边界点）
                isSurrounded = false;
                break;
              }
            }
          }
          if (isSurrounded) {           //如果九宫格全部被占用
            c.obstX = x;
            c.obstY = y;
            c.sqdist = 0;
            c.dist=0;
            c.voronoi=occupied;
            c.queueing = fwProcessed;
            data_[x][y] = c;
          } else {
            setObstacle(x,y);           //不同之处在于：将(x,y)加入addList_
          }
        }
      }
    }
  }
}

initializeEmpty()主要清空历史数据，为数组开辟内存空间，并赋初始值，将所有栅格设置为不被占用。然后，当gridMap_中的某栅格被占用，而data_中的该栅格却没被占用时，表示环境发生了变化，才需要更新栅格的信息。因为这是初始化操作，不会出现gridMap_中的某栅格不被占用、而data_中的该栅格却被占用的情况。如果一个栅格的8个邻居栅格全被占用，说明该栅格在障碍物内部，只需简单赋值，不会触发lower过程。如果8个邻居栅格没有全被占用，说明该栅格在障碍物边界上，调用setObstacle()，暂存会触发DM更新的点。

4. 添加障碍物

//要同时更新gridmap和data_
void DynamicVoronoi::occupyCell(int x, int y) {
  gridMap_[x][y] = 1;     //更新gridmap
  setObstacle(x,y);
}

//只更新data_
void DynamicVoronoi::setObstacle(int x, int y) {
  dataCell c = data_[x][y];
  if(isOccupied(x,y,c)) {               //如果data_中的(x,y)被占用
    return;
  }

  addList_.push_back(INTPOINT(x,y));    //加入addList_
  c.obstX = x;
  c.obstY = y;
  data_[x][y] = c;
}

对应文章中下图部分。