python算法

Bill78 等级 724 0 0

python算法

来源 | CSDN(ID:CSDNnews )

本文是一些机器人算法(特别是自动导航算法)的Python代码合集。

其主要特点有以下三点:选择了在实践中广泛应用的算法;依赖最少;容易阅读,容易理解每个算法的基本思想。希望阅读本文后能对你有所帮助。

前排友情提示,文章较长,建议收藏后再看。

目录

环境需求

怎样使用

本地化

  • 扩展卡尔曼滤波本地化

  • 无损卡尔曼滤波本地化

  • 粒子滤波本地化

  • 直方图滤波本地化

映射

  • 高斯网格映射

  • 光线投射网格映射

  • k均值物体聚类

  • 圆形拟合物体形状识别

SLAM

  • 迭代最近点匹配

  • EKF SLAM

  • FastSLAM 1.0

  • FastSLAM 2.0

  • 基于图的SLAM

路径规划

  • 动态窗口方式

  • 基于网格的搜索

  • 迪杰斯特拉算法

  • A*算法

  • 势场算法

  • 模型预测路径生成
  • 路径优化示例

  • 查找表生成示例

  • 状态晶格规划
  • 均匀极性采样(Uniform polar sampling)

  • 偏差极性采样(Biased polar sampling)

  • 路线采样(Lane sampling)

  • 随机路径图(PRM)规划

  • Voronoi路径图规划

  • 快速搜索随机树(RRT)

  • 基本RRT

  • RRT*

  • 基于Dubins路径的RRT

  • 基于Dubins路径的RRT*

  • 基于reeds-shepp路径的RRT*

  • Informed RRT*

  • 批量Informed RRT*

  • 三次样条规划

  • B样条规划

  • 贝济埃路径规划

  • 五次多项式规划

  • Dubins路径规划

  • Reeds Shepp路径规划

  • 基于LQR的路径规划

  • Frenet Frame中的最优路径

路径跟踪

  • 纯追迹跟踪

  • 史坦利控制

  • 后轮反馈控制

  • 线性二次regulator(LQR)转向控制

  • 线性二次regulator(LQR)转向和速度控制

项目支持

环境需求

  • Python 3.6.x

  • numpy

  • scipy

  • matplotlib

  • pandas

  • cvxpy 0.4.x

怎样使用

  1. 安装必要的库;

  2. 克隆本代码仓库;

  3. 执行每个目录下的python脚本;

  4. 如果你喜欢,则收藏本代码库:)

本地化

扩展卡尔曼滤波本地化

python算法

该算法利用扩展卡尔曼滤波器(Extended Kalman Filter, EKF)实现传感器混合本地化。

蓝线为真实路径,黑线为导航推测路径(dead reckoning trajectory),绿点为位置观测(如GPS),红线为EKF估算的路径。

红色椭圆为EKF估算的协方差。

相关阅读:

概率机器人学

http://www.probabilistic-robotics.org/

无损卡尔曼滤波本地化

python算法

该算法利用无损卡尔曼滤波器(Unscented Kalman Filter, UKF)实现传感器混合本地化。

线和点的含义与EKF模拟的例子相同。

相关阅读:

利用无差别训练过的无损卡尔曼滤波进行机器人移动本地化

https://www.researchgate.net/publication/267963417\_Discriminatively\_Trained\_Unscented\_Kalman\_Filter\_for\_Mobile\_Robot\_Localization

粒子滤波本地化

python算法

该算法利用粒子滤波器(Particle Filter, PF)实现传感器混合本地化。

蓝线为真实路径,黑线为导航推测路径(dead reckoning trajectory),绿点为位置观测(如GPS),红线为PF估算的路径。

该算法假设机器人能够测量与地标(RFID)之间的距离。

PF本地化会用到该测量结果。

相关阅读:

概率机器人学

http://www.probabilistic-robotics.org/

直方图滤波本地化

python算法

该算法是利用直方图滤波器(Histogram filter)实现二维本地化的例子。

红十字是实际位置,黑点是RFID的位置。

蓝色格子是直方图滤波器的概率位置。

在该模拟中,x,y是未知数,yaw已知。

滤波器整合了速度输入和从RFID获得距离观测数据进行本地化。

不需要初始位置。

相关阅读:

概率机器人学

http://www.probabilistic-robotics.org/

映射

高斯网格映射

本算法是二维高斯网格映射(Gaussian grid mapping)的例子。

python算法

光线投射网格映射

本算法是二维光线投射网格映射(Ray casting grid map)的例子。

python算法

k均值物体聚类

本算法是使用k均值算法进行二维物体聚类的例子。

python算法

圆形拟合物体形状识**别**

本算法是使用圆形拟合进行物体形状识别的例子。

python算法

蓝圈是实际的物体形状。

红叉是通过距离传感器观测到的点。

红圈是使用圆形拟合估计的物体形状。

SLAM

同时本地化和映射(Simultaneous Localization and Mapping,SLAM)的例子。

迭代最近点匹配

本算法是使用单值解构进行二维迭代最近点(Iterative Closest Point,ICP)匹配的例子。

它能计算从一些点到另一些点的旋转矩阵和平移矩阵。

python算法

相关阅读:

机器人运动介绍:迭代最近点算法

https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf

EKF SLAM

这是基于扩展卡尔曼滤波的SLAM示例。

蓝线是真实路径,黑线是导航推测路径,红线是EKF SLAM估计的路径。

绿叉是估计的地标。

python算法

相关阅读:

概率机器人学

http://www.probabilistic-robotics.org/

FastSLAM 1.0

这是用FastSLAM 1.0进行基于特征的SLAM的示例。

蓝线是实际路径,黑线是导航推测,红线是FastSLAM的推测路径。

红点是FastSLAM中的粒子。

黑点是地标,蓝叉是FastLSAM估算的地标位置。

python算法

相关阅读:

概率机器人学

http://www.probabilistic-robotics.org/

FastSLAM 2.0

这是用FastSLAM 2.0进行基于特征的SLAM的示例。

动画的含义与FastSLAM 1.0的情况相同。

python算法

相关阅读:

概率机器人学

http://www.probabilistic-robotics.org/

Tim Bailey的SLAM模拟

http://www-personal.acfr.usyd.edu.au/tbailey/software/slam\_simulations.htm

基于图的SLAM

这是基于图的SLAM的示例。

蓝线是实际路径。

黑线是导航推测路径。

红线是基于图的SLAM估算的路径。

黑星是地标,用于生成图的边。

python算法

相关阅读:

基于图的SLAM入门

http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf

路径规划

动态窗口方式

这是使用动态窗口方式(Dynamic Window Approach)进行二维导航的示例代码。

python算法

相关阅读:

用动态窗口方式避免碰撞

https://www.ri.cmu.edu/pub\_files/pub1/fox\_dieter\_1997\_1/fox\_dieter\_1997\_1.pdf

基于网格的搜索

迪杰斯特拉算法

这是利用迪杰斯特拉(Dijkstra)算法实现的基于二维网格的最短路径规划。

python算法

动画中青色点为搜索过的节点。

A*算法

下面是使用A星算法进行基于二维网格的最短路径规划。

python算法

动画中青色点为搜索过的节点。

启发算法为二维欧几里得距离。

势场算法

下面是使用势场算法进行基于二维网格的路径规划。

python算法

动画中蓝色的热区图显示了每个格子的势能。

相关阅读:

机器人运动规划:势能函数

https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field\_howie.pdf

模型预测路径生成

下面是模型预测路径生成的路径优化示例。

算法用于状态晶格规划(state lattice planning)。

路径优化示例

python算法

查找表生成示例

python算法

相关阅读:

用于带轮子的机器人的最优不平整地形路径生成

http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328

状态晶格规划

这个脚本使用了状态晶格规划(state lattice planning)实现路径规划。

这段代码通过模型预测路径生成来解决边界问题。

相关阅读:

用于带轮子的机器人的最优不平整地形路径生成

http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328

用于复杂环境下的高性能运动机器人导航的可行运动的状态空间采样

http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR\_08\_SS\_Sampling.pdf

均匀极性采样(Uniform polar sampling)

python算法

偏差极性采样(Biased polar sampling)

python算法

路线采样(Lane sampling)

python算法

随机路径图(PRM)规划

python算法

这个随机路径图(Probabilistic Road-Map,PRM)规划算法在图搜索上采用了迪杰斯特拉方法。

动画中的蓝点为采样点。

青色叉为迪杰斯特拉方法搜索过的点。

红线为PRM的最终路径。

相关阅读:

随机路径图

https://en.wikipedia.org/wiki/Probabilistic\_roadmap

Voronoi路径图规划

python算法

这个Voronoi路径图(Probabilistic Road-Map,PRM)规划算法在图搜索上采用了迪杰斯特拉方法。

动画中的蓝点为Voronoi点。

青色叉为迪杰斯特拉方法搜索过的点。

红线为Voronoi路径图的最终路径。

相关阅读:

机器人运动规划

https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods\_howie.pdf

快速搜索随机树(RRT)

基本RRT

python算法

这是个使用快速搜索随机树(Rapidly-Exploring Random Trees,RRT)的简单路径规划代码。

黑色圆为障碍物,绿线为搜索树,红叉为开始位置和目标位置。

RRT*

python算法

这是使用RRT*的路径规划代码。

黑色圆为障碍物,绿线为搜索树,红叉为开始位置和目标位置。

相关阅读:

最优运动规划的基于增量采样的算法

https://arxiv.org/abs/1005.0416

最优运动规划的基于采样的算法

http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf

基于Dubins路径的RRT

python算法

为汽车形机器人提供的使用RRT和dubins路径规划的路径规划算法。

基于Dubins路径的RRT*

python算法

为汽车形机器人提供的使用RRT*和dubins路径规划的路径规划算法。

基于reeds-shepp路径的RRT*

python算法

为汽车形机器人提供的使用RRT*和reeds shepp路径规划的路径规划算法。

Informed RRT*

python算法

这是使用Informed RRT*的路径规划代码。

青色椭圆为Informed RRT*的启发采样域。

相关阅读:

Informed RRT*:通过对可接受的椭球启发的直接采样实现最优的基于采样的路径规划

https://arxiv.org/pdf/1404.2334.pdf

批量Informed RRT*

python算法

这是使用批量Informed RRT*的路径规划代码。

相关阅读:

批量Informed树(BIT*):通过对隐含随机几何图形进行启发式搜索实现基于采样的最优规划

https://arxiv.org/abs/1405.5848

闭合回路RRT*

使用闭合回路RRT*(Closed loop RRT*)实现的基于车辆模型的路径规划。

python算法

这段代码里,转向控制用的是纯追迹算法(pure-pursuit algorithm)。

速度控制采用了PID。

相关阅读:

使用闭合回路预测在复杂环境内实现运动规划

http://acl.mit.edu/papers/KuwataGNC08.pdf)

应用于自动城市驾驶的实时运动规划

http://acl.mit.edu/papers/KuwataTCST09.pdf

[1601.06326]采用闭合回路预测实现最优运动规划的基于采样的算法

https://arxiv.org/abs/1601.06326

LQR-RRT*

这是个使用LQR-RRT*的路径规划模拟。

LQR局部规划采用了双重积分运动模型。

python算法

相关阅读:

LQR-RRT*:使用自动推导扩展启发实现最优基于采样的运动规划

http://lis.csail.mit.edu/pubs/perez-icra12.pdf

MahanFathi/LQR-RRTstar:LQR-RRT*方法用于单摆相位中的随机运动规划

https://github.com/MahanFathi/LQR-RRTstar

三次样条规划

这是段三次路径规划的示例代码。

这段代码根据x-y的路点,利用三次样条生成一段曲率连续的路径。

每个点的指向角度也可以用解析的方式计算。

python算法

python算法

python算法

B样条规划

python算法

这是段使用B样条曲线进行规划的例子。

输入路点,它会利用B样条生成光滑的路径。

第一个和最后一个路点位于最后的路径上。

相关阅读:

B样条

https://en.wikipedia.org/wiki/B-spline

Et**a^**3样条路径规划

python算法

这是使用Eta ^ 3样条曲线的路径规划。

相关阅读:

\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots

https://ieeexplore.ieee.org/document/4339545/

贝济埃路径规划

贝济埃路径规划的示例代码。

根据四个控制点生成贝济埃路径。

python算法

改变起点和终点的偏移距离,可以生成不同的贝济埃路径:

python算法

相关阅读:

根据贝济埃曲线为自动驾驶汽车生成曲率连续的路径

http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf

五次多项式规划

利用五次多项式进行路径规划。

python算法

它能根据五次多项式计算二维路径、速度和加速度。

相关阅读:

用于Agv In定位的局部路径规划和运动控制

http://ieeexplore.ieee.org/document/637936/

Dubins路径规划

Dubins路径规划的示例代码。

python算法

相关阅读:

Dubins路径

https://en.wikipedia.org/wiki/Dubins\_path

Reeds Shepp路径规划

Reeds Shepp路径规划的示例代码。

python算法

相关阅读:

15.3.2 Reeds-Shepp曲线

http://planning.cs.uiuc.edu/node822.html

用于能前进和后退的汽车的最优路径

https://pdfs.semanticscholar.org/932e/c495b1d0018fd59dee12a0bf74434fac7af4.pdf

ghliu/pyReedsShepp:实现Reeds Shepp曲线

https://github.com/ghliu/pyReedsShepp

基于LQR的路径规划

为双重积分模型使用基于LQR的路径规划的示例代码。

python算法

Frenet Frame中的最优路径

python算法

这段代码在Frenet Frame中生成最优路径。

青色线为目标路径,黑色叉为障碍物。

红色线为预测的路径。

相关阅读:

Frenet Frame中的动态接到场景中的最优路径生成

https://www.researchgate.net/profile/Moritz\_Werling/publication/224156269\_Optimal\_Trajectory\_Generation\_for\_Dynamic\_Street\_Scenarios\_in\_a\_Frenet\_Frame/links/54f749df0cf210398e9277af.pdf

Frenet Frame中的动态接到场景中的最优路径生成

https://www.youtube.com/watch?v=Cj6tAQe7UCY

路径跟踪

姿势控制跟踪

这是姿势控制跟踪的模拟。

python算法

相关阅读:

Robotics, Vision and Control - Fundamental Algorithms In MATLAB® Second, Completely Revised, Extended And Updated Edition | Peter Corke | Springer

https://www.springer.com/us/book/9783319544120

纯追迹跟踪

使用纯追迹(pure pursuit)转向控制和PID速度控制的路径跟踪模拟。

python算法

红线为目标路线,绿叉为纯追迹控制的目标点,蓝线为跟踪路线。

相关阅读:

城市中的自动驾驶汽车的运动规划和控制技术的调查

https://arxiv.org/abs/1604.07446

史坦利控制

使用史坦利(Stanley)转向控制和PID速度控制的路径跟踪模拟。

python算法

相关阅读:

史坦利:赢得DARPA大奖赛的机器人

http://robots.stanford.edu/papers/thrun.stanley05.pdf

用于自动驾驶机动车路径跟踪的自动转向方法

https://www.ri.cmu.edu/pub\_files/2009/2/Automatic\_Steering\_Methods\_for\_Autonomous\_Automobile\_Path\_Tracking.pdf

后轮反馈控制

利用后轮反馈转向控制和PID速度控制的路径跟踪模拟。

python算法

相关阅读:

城市中的自动驾驶汽车的运动规划和控制技术的调查

https://arxiv.org/abs/1604.07446

线性二次regulator(LQR)转向控制

使用LQR转向控制和PID速度控制的路径跟踪模拟。

python算法

相关阅读:

ApolloAuto/apollo:开源自动驾驶平台

https://github.com/ApolloAuto/apollo

线性二次regulator(LQR)转向和速度控制

使用LQR转向和速度控制的路径跟踪模拟。

python算法

相关阅读:

完全自动驾驶:系统和算法 - IEEE会议出版物

http://ieeexplore.ieee.org/document/5940562/

模型预测速度和转向控制

使用迭代线性模型预测转向和速度控制的路径跟踪模拟。

python算法

这段代码使用了cxvxpy作为最优建模工具。

相关阅读:

车辆动态和控制 | Rajesh Rajamani | Springer

http://www.springer.com/us/book/9781461414322

MPC课程资料 - MPC Lab @ UC-Berkeley

http://www.mpc.berkeley.edu/mpc-course-material

项目支持

可以通过Patreon(https://www.patreon.com/myenigma)对该项目进行经济支持。

如果你在Patreon上支持该项目,则可以得到关于本项目代码的邮件技术支持。

本文作者包括有Atsushi Sakai (@Atsushi_twi),Daniel Ingram,Joe Dinius,Karan Chawla,Antonin RAFFIN,Alexis Paques。

原文:

https://atsushisakai.github.io/PythonRobotics/#what-is-this

作者:AtsushiSakai,日本机器人工程师,从事自动驾驶技术开发,精通C++、ROS、MATLAB、Python、Vim和Robotics。

译者:弯月,责编:郭芮

精彩推荐

python算法

推荐阅读:

python算法

本文转自 https://blog.csdn.net/weixin_42232219/article/details/87746070,如有侵权,请联系删除。

收藏
评论区

相关推荐

Python的环境搭建和下载
Python是一个跨平台、可移植的编程语言,因此可在windows、Linux和Mac OS X系统中安装使用。 安装完成后,你会得到Python解释器环境,可以通过终端输入python命令查看本地是否已经按照python以及python版本。这里有一点需要注意的是,如果没有将python的安装目录添加到环境变量中,会报错(python不是内部命令或外部命
Python编程基础(快速入门必看
Python编程基础一、Python语言基本语法 Python是一
python文件的第一行 #!/usr/bin/python3 是什么意思?
python文件的第一行代码通常在脚本语言的第一行会看到: !/usr/bin/env python或 !/usr/bin/python 首先要确定的一点是它不是注释。这两句话的目的都是指出你的python文件用什么可执行程序去运行它。1. !/usr/bin/python 是告诉操作系统执行这个脚本的时候,调用 /usr/bin 下的 python 解释
Ubuntu 常用命令记录
一、Python相关设置 ------------ 修改Python默认版本 ------------ cd /usr/bin sudo rm -rf python sudo ln -s /usr/bin/python3 /usr/bin/python 检查是否设置成功: python -V 安
Atom的python插件和常用插件
python: simplified-chinese-menu:中文汉化(英文差的) 代码高亮:Atom自带 自动补全:autocomplete-python 语法检查:linter-flake8 定义跳转:python-tools 代码运行:atom-runner (只能输出,不能输入),atom-python-run(Windows,可以输入
CentOS升级Python到2.7版本
查看python的版本 python -V Python 2.4.3 1.先安装GCC yum -y install gcc 2.下载Python-2.7.2 wget http://python.org/ftp/python/2.7.2/Python-2.7.2.tar.bz2 3.解压Python-2.7.2
Django学习笔记
1\. django-admin.py startproject mysite 2\. python manage.py help 3\. python manage.py runserver 4\. python 搜索路径为sys.path,运行时可追加目录至sys.path 5\. python模板使用: \>>> from django im
FreeBSD python安装MySQL
fetch https://pypi.python.org/packages/source/M/MySQL-python/MySQL-python-1.2.4.zip unzip MySQL-python-1.2.4.zip cd MySQL-python-1.2.4 python setup.py install ln -s /usr/loca
Linux下Python编程
linux下python编程可以使用sublime-text3来作为编辑器,很方便,而且linux默认就是有python2和python3环境安装的,我们只要安装sublime-text3即可进行python编程 sudo update-alternatives --install /usr/bin/python python /usr/bin/p
Python 3 教程
Python 3 教程 =========== ![python3](https://www.runoob.com/wp-content/uploads/2014/05/python3.png) Python 的 3.0 版本,常被称为 Python 3000,或简称 Py3k。相对于 Python 的早期版本,这是一个较大的升级。为了不带入过多的累赘,
Python List数据的遍历
方式一: <!-- lang: python --> app_list = [1234, 5677, 8899] <!-- lang: python --> for app_id in app_list: <!-- lang: python --> print app_id 输出: 123
Python中的参数传递与解析
Python传递命令行参数 ============= Python的命令行参数传递和C语言类似,都会把命令行参数保存到argv的变量中。对于python而言,argv是sys模块中定义的一个list。与C语言不同的是,python中并没有定义argc,要获得参数的个数,需要使用len(sys.argv) 当用户使用'python -c "command
Python环境搭建—安利Python小白的Python和Pycharm安装详细教程
人生苦短,我用Python。众所周知,Python目前越来越火,学习Python的小伙伴也越来越多。最近看到群里的小伙伴经常碰到不会安装Python或者不知道去哪下载Python安装包等系列问题,为了方便大家学习Python,小编整理了一套Python和Pycharm安装详细教程,只要大家按照这个步骤来,就可以轻松的搞定Python和Pycharm的安装了。
Python解释器和IPython
简介 == 今天给大家介绍一下Python的一个功能非常强大的解释器IPython。虽然Python本身自带解释器,但是相对而言IPython的功能更加的强大。 Python解释器 ========= Python是自带解释器的,我们在命令行输入python即可进入python的解释器环境: $> pythonPython 2.7.15 (d
Python进阶丨如何创建你的第一个Python元类?
> **摘要:**通过本文,将深入讨论Python元类,其属性,如何以及何时在Python中使用元类。 Python元类设置类的行为和规则。元类有助于修改类的实例,并且相当复杂,是Python编程的高级功能之一。通过本文,将深入讨论Python元类,其属性,如何以及何时在Python中使用元类。本文介绍以下概念: * * 什么是Python元类?