路径规划算法: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

相关文章

Python Transformers库(NLP处理库)案例代码讲解

《PythonTransformers库(NLP处理库)案例代码讲解》本文介绍transformers库的全面讲解,包含基础知识、高级用法、案例代码及学习路径,内容经过组织,适合不同阶段的学习者,对... 目录一、基础知识1. Transformers 库简介2. 安装与环境配置3. 快速上手示例二、核心模

openCV中KNN算法的实现

《openCV中KNN算法的实现》KNN算法是一种简单且常用的分类算法,本文主要介绍了openCV中KNN算法的实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的... 目录KNN算法流程使用OpenCV实现KNNOpenCV 是一个开源的跨平台计算机视觉库,它提供了各

C++ vector的常见用法超详细讲解

《C++vector的常见用法超详细讲解》:本文主要介绍C++vector的常见用法,包括C++中vector容器的定义、初始化方法、访问元素、常用函数及其时间复杂度,通过代码介绍的非常详细,... 目录1、vector的定义2、vector常用初始化方法1、使编程用花括号直接赋值2、使用圆括号赋值3、ve

Java调用C++动态库超详细步骤讲解(附源码)

《Java调用C++动态库超详细步骤讲解(附源码)》C语言因其高效和接近硬件的特性,时常会被用在性能要求较高或者需要直接操作硬件的场合,:本文主要介绍Java调用C++动态库的相关资料,文中通过代... 目录一、直接调用C++库第一步:动态库生成(vs2017+qt5.12.10)第二步:Java调用C++

springboot+dubbo实现时间轮算法

《springboot+dubbo实现时间轮算法》时间轮是一种高效利用线程资源进行批量化调度的算法,本文主要介绍了springboot+dubbo实现时间轮算法,文中通过示例代码介绍的非常详细,对大家... 目录前言一、参数说明二、具体实现1、HashedwheelTimer2、createWheel3、n

Python基础文件操作方法超详细讲解(详解版)

《Python基础文件操作方法超详细讲解(详解版)》文件就是操作系统为用户或应用程序提供的一个读写硬盘的虚拟单位,文件的核心操作就是读和写,:本文主要介绍Python基础文件操作方法超详细讲解的相... 目录一、文件操作1. 文件打开与关闭1.1 打开文件1.2 关闭文件2. 访问模式及说明二、文件读写1.

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 的缓