目录
- 0 专栏介绍
- 1 什么是维诺图？
- 2 计算几何中的维诺图
- 3 广义维诺图
- 3.1 定义
- 3.2 算法原理
- 4 维诺图实现
- 4.1 C++实现
- 4.2 Python实现
- 4.3 Matlab实现
0 专栏介绍
🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。
🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法
1 什么是维诺图？
维诺图(Voronoi Diagram)，也称为泰森多边形(Thiessen Polygon)，是一种用于将空间分割为一组区域的图形化方法，其中每个区域都由一个特定点（种子点）控制，使得每个点到其控制的种子点最近。换句话说，维诺图将空间分成了一组以种子点为中心的多边形，每个多边形内的点都离相应的种子点最近。
维诺图的应用涵盖了多个领域，包括计算机图形学、地理信息系统、计算机视觉等，举几个例子
- 地理信息系统： 维诺图可以用来表示地理空间中的区域划分，例如在城市规划中确定各个服务设施的服务范围，或者在地理分析中确定每个点距离最近的设施或资源。
- 计算机图形学： 维诺图在计算机图形学中被用来生成类似水彩画风格的渲染效果，或者用于粒子模拟和自然景观生成等。
- 无线通信： 维诺图可以用来优化无线信号的传输范围和覆盖范围，帮助设计无线网络布局，以提高通信效率和覆盖范围。
- 城市规划： 维诺图可以用于确定城市中不同区域的辐射中心，例如交通路口、公共设施等，有助于优化城市的资源分配和规划。
- …
本节我们重点关注计算几何与运动规划中维诺图的应用
2 计算几何中的维诺图
在计算几何中，维诺图的构建分为两步：
-
对离散点集构建Delaunay三角网；
Delaunay三角网是三角剖分的一种。离散点集的Delaunay三角网是唯一的，且没有任何点在三角形的外接圆内部。
-
计算Delaunay三角网各三角形外接圆圆心，连接各圆心得到维诺图
该算法可以从PyDelaunay中获取
# initialize the TIN builder with the test range of X=[0,100] and Y=[0,100]
builder = TinBuilder(0.0, 100.0, 0.0, 100.0)
# randomly generate a set of points
sites = []
num = 100
i = 0
while i < num:
site = Site(random.randint(0,100), random.randint(0,100))
print site.x, site.y
site.sitenum = i
sites.append(site)
edge = builder.insertSite(site)
if (edge != None):
i += 1
# output the TIN results
draw_delaunay(builder, sites)
draw_voronoi(builder, sites)
plt.plot([p.x for p in sites], [p.y for p in sites], 'ro')
plt.axis([0,100,0,100])
plt.show()
3 广义维诺图
3.1 定义
在机器人领域中，维诺图作为一种栅格图分解方法用于自主导航任务。与计算几何的概念类似，令离散点集为障碍栅格，则定义规划空间中离最近的两个障碍物具有相同距离的点集为广义维诺图(Generalized Voronoi Diagram, GVD)。GVD通过对空间划分有效减少了路径搜索维度，且沿着GVD边缘移动可确保在穿越障碍物时具有最大的安全间隙
3.2 算法原理
在实现过程中，GVD应用了称为距离图(Distance Maps, DMs)的数据结构。DMs矩阵的每个元素存储到最近障碍物的距离，由于内存检索为常数时间复杂度，DMs降低了碰撞检测、路径规划的遍历成本，常见的距离图为欧式距离图(Euclidean Distance Maps, EDMs)。
文献《Improved updating of Euclidean distance maps and Voronoi diagrams》提出了在给定障碍物地图时，计算与更新GVD的高效方法。算法主要分为：
-
距离图的纹波更新
借鉴D*算法的定义，将栅格分为
RAISE和LOWER两种状态，前者负责传播障碍移除信息，后者负责传播距离更新信息。如图(a)所示，移除障碍时，以原障碍为中心向邻域扩散RAISE状态直至碰到最近障碍物，此后从碰到的障碍物开始反向传播LOWER信息以更新距离场；新增障碍时，由于障碍附近的栅格可直接计算离障碍物的最近距离，因此可以直接通过LOWER向外扩散更新距离信息。
-
广义维诺图的构建与裁剪
在
LOWER状态的扩散过程中，设扩展节点距离障碍物的原最近距离为，此时距扩散源障碍的距离为
。若
，则显然要更新距离场
；若
则纹波扩散停止；若
，此时如果扩展节点已知，则可以将该节点记录到GVD中，如果扩展节点处于未知区域，则更新该节点为已知并重新加入
LOWER传播过程。所以当距离图更新完毕后，可以同时得到一系列满足在连续空间中，如果一个点到最近两个障碍物的距离相等，则该点是GVD的一部分。然而这个条件不能直接应用于离散栅格地图，受有限分辨率的限制，算法需要计算与维诺线关联的离散点作为GVD的候选栅格。接着，对候选栅格进行裁剪
4 维诺图实现
4.1 C++实现
C++核心代码如下
void DynamicVoronoi::update(bool updateRealDist) {
commitAndColorize(updateRealDist);
while (!open.empty()) {
INTPOINT p = open.pop();
int x = p.x;
int y = p.y;
dataCell c = data[x][y];
if(c.queueing==fwProcessed) continue;
if (c.needsRaise) {
// RAISE
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;
dataCell nc = data[nx][ny];
if (nc.obstX!=invalidObstData && !nc.needsRaise) {
if(!isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])) {
open.push(nc.sqdist, INTPOINT(nx,ny));
nc.queueing = fwQueued;
nc.needsRaise = true;
nc.obstX = invalidObstData;
nc.obstY = invalidObstData;
if (updateRealDist) nc.dist = INFINITY;
nc.sqdist = INT_MAX;
data[nx][ny] = nc;
} else {
if(nc.queueing != fwQueued){
open.push(nc.sqdist, INTPOINT(nx,ny));
nc.queueing = fwQueued;
data[nx][ny] = nc;
}
}
}
}
}
c.needsRaise = false;
c.queueing = bwProcessed;
data[x][y] = c;
}
else if (c.obstX != invalidObstData && isOccupied(c.obstX,c.obstY,data[c.obstX][c.obstY])) {
// LOWER
c.queueing = fwProcessed;
c.voronoi = occupied;
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;
dataCell nc = data[nx][ny];
if(!nc.needsRaise) {
int distx = nx-c.obstX;
int disty = ny-c.obstY;
int newSqDistance = distx*distx + disty*disty;
bool overwrite = (newSqDistance < nc.sqdist);
if(!overwrite && newSqDistance==nc.sqdist) {
if (nc.obstX == invalidObstData || isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])==false) overwrite = true;
}
if (overwrite) {
open.push(newSqDistance, INTPOINT(nx,ny));
nc.queueing = fwQueued;
if (updateRealDist) {
nc.dist = sqrt((double) newSqDistance);
}
nc.sqdist = newSqDistance;
nc.obstX = c.obstX;
nc.obstY = c.obstY;
} else {
checkVoro(x,y,nx,ny,c,nc);
}
data[nx][ny] = nc;
}
}
}
}
data[x][y] = c;
}
}
4.2 Python实现
python核心代码如下
from scipy.spatial import Voronoi
vor = Voronoi(np.array(list(self.env.obstacles)))
vx_list = [ix for [ix, _] in vor.vertices] + [self.start.x, self.goal.x]
vy_list = [iy for [_, iy] in vor.vertices] + [self.start.y, self.goal.y]
sample_num = len(vx_list)
expand = [Node((vx_list[i], vy_list[i])) for i in range(sample_num)]
4.3 Matlab实现
matlab核心代码如下
% construct Voronoi diagram
[ox, oy] = find(map == 2);
[vx, vy] = voronoi(oy, ox);
start(:, [1 2]) = start(:, [2 1]);
goal(:, [1 2]) = goal(:, [2 1]);
% Voronoi diagram filter
index_x = intersect(find(vx(1, :) > 0 & vx(1, :) < x_range), ...
find(vx(2, :) > 0 & vx(2, :) < x_range));
index_y = intersect(find(vy(1, :) > 0 & vy(1, :) < y_range), ...
find(vy(2, :) > 0 & vy(2, :) < y_range));
index = intersect(index_x, index_y);
vx = vx(:, index); vy = vy(:, index);
vd_vertex = [];
EXPAND = [];
for i = 1:length(index)
node1 = [vx(1, i), vy(1, i)];
node2 = [vx(2, i), vy(2, i)];
if ~all(node1 == node2) && ~is_collision(node1, node2, map, -1, resolution)
EXPAND = [EXPAND, [vx(:, i); vy(:, i)]];
vd_vertex = [vd_vertex; [vx(:, i), vy(:, i)]];
end
end
完整工程代码请联系下方博主名片获取
🔥 更多精彩专栏：
- 《ROS从入门到精通》
- 《Pytorch深度学习实战》
- 《机器学习强基计划》
- 《运动规划实战精讲》
- …
文章出处登录后可见！
