路径规划算法:Voronoi Planner讲解

2024-04-06 21:20

本文主要是介绍路径规划算法:Voronoi Planner讲解,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

路径规划算法:Voronoi Planner讲解

image

附赠自动驾驶学习资料和量产经验:链接

Voronoi Diagram(也称作Dirichlet tessellation)是由俄国数学家Georgy Voronoy提出的一种空间分割算法。它通过一系列的种子节点(Seed Points)将空间切分为许多子区域,每个子区域被称为一个Cell,每个Cell中的所有点到当前Cell中的种子节点(Seed Points)的距离小于到其它所有种子节点(Seed Points)的距离。

image

图片来源: https://www.youtube.com/watch?v=7eCrHAv6sYY

image

每个Cell中包含的都是距离当前Cell距离最近的所有点,因此Cell的边界就是距离种子点(Seed Points)最远的点的集合。利用Voronoi Diagram的这个特性,将障碍物的边界当做种子点(Seed Points),那么Cell的边界就是远离所有障碍物的可行驶路径。

Voronoi Planner最大化的利用了障碍物之间的空隙,确保生成的路径是最大程度远离所有障碍物的安全行驶路径。

image

图片来源:https://natanaso.github.io/ece276b2018/ref/ECE276B_5_ConfigurationSpace.pdf

1、使用Voronoi Diagram进行路径规划

下图是一所大学校园的地图,图中包含各种多变形的障碍物,我们可以使用使用Voronoi Planner实现在地图中查找一条安全路径,最大程度的避开障碍物。

image

the northern half of Columbia's Morningside Campus.图片来源:https://www.cs.columbia.edu/~pblaer/projects/path_planner/

为了实现Voronoi路径规划,首先用一系列的离散点集序列组成的小线段模拟逼近多边形障碍物的每个边。

image

The points that approximate thepolygonal obstacles. 图片来源:https://www.cs.columbia.edu/~pblaer/projects/path_planner/

然后使用这些近似的离散点作为输入,使用Voronoi构造算法构造Voronoi Diagram。

image

The points that approximate thepolygonal obstacles. 图片来源:https://www.cs.columbia.edu/~pblaer/projects/path_planner/

Voronoi diagram构造完成之后,消除顶点包含在障碍物或者与障碍物相交的Voronoi Edge,剩下的Voronoi Edge就构成了避开所有障碍物的可行驶路径集合。

image

The points that approximate thepolygonal obstacles. 图片来源:https://www.cs.columbia.edu/~pblaer/projects/path_planner/

最后,将Voronoi Edge转化为Grahp结构,将机器人的起点位置和终点位置关联到最近的Voronoi Edge,然后通过图搜索算法(Dijkstra等)就可以生成一条从起点到终点的安全行驶路线。

2、Voronio Planner VS Sample Planner

从下图的对比可以看出,Voronoi Planner规划的路径的特点是尽量的远离障碍物。

image

图片来源:Local and Global Motion Planning for Unmanned Surface Vehicle

image

图片来源:Local and Global Motion Planning for Unmanned Surface Vehicle

3、梯度下降的路径平滑算法

同基于采样的运动规划生成的曲线一样,Voronio Planner生成的曲线都是不平滑的折线,所以需要对路径进行平滑操作,平滑的方法也比较多,今天先介绍其中一种。

3.1 问题定义

如下图所示,s表示运动规划的起点,e表示运动规划终点,斜线填充的网格表示障碍物位置,蓝色的线为运动规划算法(RRT、Voronoi etc.)规划出的路线,曲折不平;红色为平滑后的运动曲线,对车辆的实际行驶比较友好。

image

image

image

3.2 算法实现

上图代码一个5x5的网格地图,红色圆圈代表一条从(0,0)到(4,4)的规划路线,下Python面代码演示了如何由这条路线生成一条平滑路线。

image

from math import *path = [[0, 0],[0, 1],[0, 2],[1, 2],[2, 2],[3, 2],[4, 2],[4, 3],[4, 4]]def smooth(path, weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001):# Make a deep copy of path into newpathnewpath = [[0 for col in range(len(path[0]))] for row in range(len(path))]for i in range(len(path)):for j in range(len(path[0])):newpath[i][j] = path[i][j]change = tolerancewhile change >= tolerance:change = 0for i in range(1,len(path) - 1):for j in range(len(path[0])):d1 = weight_data * (path[i][j] - newpath[i][j])d2 = weight_smooth * (newpath[i-1][j] + newpath[i+1][j] - 2 * newpath[i][j])change += abs(d1 + d2)newpath[i][j] += d1 + d2return newpath newpath = smooth(path)for i in range(len(path)):print('['+ ', '.join('%.3f'%x for x in path[i]) +']> ['+', '.join('%.3f'%x for x in newpath[i]) +']')

平滑后的路径输出结果如下:

[0.000, 0.000]> [0.000, 0.000]
[0.000, 1.000]> [0.021, 0.979]
[0.000, 2.000]> [0.149, 1.851]
[1.000, 2.000]> [1.021, 1.979]
[2.000, 2.000]> [2.000, 2.000]
[3.000, 2.000]> [2.979, 2.021]
[4.000, 2.000]> [3.851, 2.149]
[4.000, 3.000]> [3.979, 3.021]
[4.000, 4.000]> [4.000, 4.000]

平滑算法的实际应用效果如下:

image

图片来源:Local and Global Motion Planning for Unmanned Surface Vehicle

相关代码

1、Boost Voronio Diagram。(https://www.boost.org/doc/libs/1_60_0/libs/polygon/doc/voronoi_diagram.htm)

2、Scipy Spatial Voronoi(https://docs.scipy.org/doc/scipy-0.18.1/reference/generated/scipy.spatial.Voronoi.html)

3、Voronoi Planner的代码实现可以参考:

https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/VoronoiRoadMap/voronoi_road_map.py

参考链接

1、Boost Voronio Diagram。(https://www.boost.org/doc/libs/1_60_0/libs/polygon/doc/voronoi_diagram.htm)

2、Robot Path Planning Using Generalized Voronoi Diagrams(https://www.cs.columbia.edu/~pblaer/projects/path_planner/)

3、Local and Global Motion Planning for Unmanned Surface Vehicle,Roman Fedorenko, Boris Gurenko

这篇关于路径规划算法:Voronoi Planner讲解的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/880859

相关文章

C# WinForms存储过程操作数据库的实例讲解

《C#WinForms存储过程操作数据库的实例讲解》:本文主要介绍C#WinForms存储过程操作数据库的实例,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一、存储过程基础二、C# 调用流程1. 数据库连接配置2. 执行存储过程(增删改)3. 查询数据三、事务处

SpringBoot实现MD5加盐算法的示例代码

《SpringBoot实现MD5加盐算法的示例代码》加盐算法是一种用于增强密码安全性的技术,本文主要介绍了SpringBoot实现MD5加盐算法的示例代码,文中通过示例代码介绍的非常详细,对大家的学习... 目录一、什么是加盐算法二、如何实现加盐算法2.1 加盐算法代码实现2.2 注册页面中进行密码加盐2.

Java时间轮调度算法的代码实现

《Java时间轮调度算法的代码实现》时间轮是一种高效的定时调度算法,主要用于管理延时任务或周期性任务,它通过一个环形数组(时间轮)和指针来实现,将大量定时任务分摊到固定的时间槽中,极大地降低了时间复杂... 目录1、简述2、时间轮的原理3. 时间轮的实现步骤3.1 定义时间槽3.2 定义时间轮3.3 使用时

Linux修改pip和conda缓存路径的几种方法

《Linux修改pip和conda缓存路径的几种方法》在Python生态中,pip和conda是两种常见的软件包管理工具,它们在安装、更新和卸载软件包时都会使用缓存来提高效率,适当地修改它们的缓存路径... 目录一、pip 和 conda 的缓存机制1. pip 的缓存机制默认缓存路径2. conda 的缓

C++快速排序超详细讲解

《C++快速排序超详细讲解》快速排序是一种高效的排序算法,通过分治法将数组划分为两部分,递归排序,直到整个数组有序,通过代码解析和示例,详细解释了快速排序的工作原理和实现过程,需要的朋友可以参考下... 目录一、快速排序原理二、快速排序标准代码三、代码解析四、使用while循环的快速排序1.代码代码1.由快

Windows系统下如何查找JDK的安装路径

《Windows系统下如何查找JDK的安装路径》:本文主要介绍Windows系统下如何查找JDK的安装路径,文中介绍了三种方法,分别是通过命令行检查、使用verbose选项查找jre目录、以及查看... 目录一、确认是否安装了JDK二、查找路径三、另外一种方式如果很久之前安装了JDK,或者在别人的电脑上,想

Python中Windows和macOS文件路径格式不一致的解决方法

《Python中Windows和macOS文件路径格式不一致的解决方法》在Python中,Windows和macOS的文件路径字符串格式不一致主要体现在路径分隔符上,这种差异可能导致跨平台代码在处理文... 目录方法 1:使用 os.path 模块方法 2:使用 pathlib 模块(推荐)方法 3:统一使

一文教你解决Python不支持中文路径的问题

《一文教你解决Python不支持中文路径的问题》Python是一种广泛使用的高级编程语言,然而在处理包含中文字符的文件路径时,Python有时会表现出一些不友好的行为,下面小编就来为大家介绍一下具体的... 目录问题背景解决方案1. 设置正确的文件编码2. 使用pathlib模块3. 转换路径为Unicod

如何通过Golang的container/list实现LRU缓存算法

《如何通过Golang的container/list实现LRU缓存算法》文章介绍了Go语言中container/list包实现的双向链表,并探讨了如何使用链表实现LRU缓存,LRU缓存通过维护一个双向... 目录力扣:146. LRU 缓存主要结构 List 和 Element常用方法1. 初始化链表2.

Java集合中的List超详细讲解

《Java集合中的List超详细讲解》本文详细介绍了Java集合框架中的List接口,包括其在集合中的位置、继承体系、常用操作和代码示例,以及不同实现类(如ArrayList、LinkedList和V... 目录一,List的继承体系二,List的常用操作及代码示例1,创建List实例2,增加元素3,访问元