点击上方“小白学视觉”,选择加"星标"或“置顶

重磅干货,第一时间送达

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

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

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

目录

一、环境需求

二、怎样使用

三、本地化

    3.1 扩展卡尔曼滤波本地化

    3.2 无损卡尔曼滤波本地化

    3.3 粒子滤波本地化

    3.4 直方图滤波本地化

四、映射

    4.1 高斯网格映射

    4.2 光线投射网格映射

    4.3 k均值物体聚类

    4.4 圆形拟合物体形状识别

五、SLAM

    5.1 迭代最近点匹配

    5.2 EKF SLAM

    5.3 FastSLAM 1.0

    5.4 FastSLAM 2.0

    5.5 基于图的SLAM

六、路径规划

    6.1 动态窗口方式

    6.2 基于网格的搜索

  • 迪杰斯特拉算法

  • A*算法

  • 势场算法

    6.3 模型预测路径生成

  • 路径优化示例

  • 查找表生成示例

    6.4 状态晶格规划

  • 均匀极性采样(Uniform polar sampling)

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

  • 路线采样(Lane sampling)

    6.5 随机路径图(PRM)规划

    6.6 Voronoi路径图规划

    6.7 快速搜索随机树(RRT)

  • 基本RRT

  • RRT*

  • 基于Dubins路径的RRT

  • 基于Dubins路径的RRT*

  • 基于reeds-shepp路径的RRT*

  • Informed RRT*

  • 批量Informed RRT*

  • 闭合回路RRT*

  • LQR-RRT*

6.8 三次样条规划

6.9 B样条规划

6.10 Eta^3样条路径规划

6.11 贝济埃路径规划

6.12 五次多项式规划

6.13 Dubins路径规划

6.14 Reeds Shepp路径规划

6.15 基于LQR的路径规划

6.16 Frenet Frame中的最优路径

七、路径跟踪

7.1 姿势控制跟踪

7.2 纯追迹跟踪

7.3 史坦利控制

7.4 后轮反馈控制

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

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

7.7 模型预测速度和转向控制

八、项目支持

一、环境需求

  • Python 3.6.x

  • numpy

  • scipy

  • matplotlib

  • pandas

  • cvxpy 0.4.x

二、怎样使用

  1. 安装必要的库;

  2. 克隆本代码仓库;

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

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

三、本地化

3.1 扩展卡尔曼滤波本地化

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

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

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

相关阅读:

概率机器人学

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

3.2 无损卡尔曼滤波本地化

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

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

相关阅读:

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

https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization

3.3 粒子滤波本地化

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

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

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

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

相关阅读:

概率机器人学

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

3.4 直方图滤波本地化

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

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

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

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

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

不需要初始位置。

相关阅读:

概率机器人学

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

四、映射

4.1 高斯网格映射

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

4.2 光线投射网格映射

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

4.3 k均值物体聚类

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

4.4 圆形拟合物体形状识别

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

蓝圈是实际的物体形状。

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

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

五、SLAM

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

5.1 迭代最近点匹配

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

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

相关阅读:

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

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

5.2 EKF SLAM

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

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

绿叉是估计的地标。

相关阅读:

概率机器人学

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

5.3 FastSLAM 1.0

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

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

红点是FastSLAM中的粒子。

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

相关阅读:

概率机器人学

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

5.4 FastSLAM 2.0

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

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

相关阅读:

概率机器人学

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

Tim Bailey的SLAM模拟

http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm

5.5 基于图的SLAM

这是基于图的SLAM的示例。

蓝线是实际路径。

黑线是导航推测路径。

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

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

相关阅读:

基于图的SLAM入门

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

六、路径规划

6.1 动态窗口方式

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

相关阅读:

用动态窗口方式避免碰撞

https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf

6.2 基于网格的搜索

  • 迪杰斯特拉算法

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

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

  • A*算法

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

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

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

  • 势场算法

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

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

相关阅读:

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

https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf

6.3 模型预测路径生成

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

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

  • 路径优化示例

  • 查找表生成示例

相关阅读:

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

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

6.4 状态晶格规划

这个脚本使用了状态晶格规划(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)

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

  • 路线采样(Lane sampling)

6.5 随机路径图(PRM)规划

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

动画中的蓝点为采样点。

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

红线为PRM的最终路径。

相关阅读:

随机路径图

https://en.wikipedia.org/wiki/Probabilistic_roadmap

6.6 Voronoi路径图规划

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

动画中的蓝点为Voronoi点。

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

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

相关阅读:

机器人运动规划

https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf

6.7 快速搜索随机树(RRT)

  • 基本RRT

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

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

  • RRT*

这是使用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

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

  • 基于Dubins路径的RRT*

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

  • 基于reeds-shepp路径的RRT*

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

  • Informed RRT*

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

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

相关阅读:

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

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

  • 批量Informed RRT*

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

相关阅读:

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

https://arxiv.org/abs/1405.5848

  • 闭合回路RRT*

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

这段代码里,转向控制用的是纯追迹算法(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局部规划采用了双重积分运动模型。

相关阅读:

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

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

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

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

6.8 三次样条规划

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

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

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

6.9 B样条规划

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

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

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

相关阅读:

B样条

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

6.10 Eta^3样条路径规划

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

相关阅读:

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

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

6.11 贝济埃路径规划

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

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

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

相关阅读:

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

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

6.12 五次多项式规划

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

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

相关阅读:

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

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

6.13 Dubins路径规划

Dubins路径规划的示例代码。

相关阅读:

Dubins路径

https://en.wikipedia.org/wiki/Dubins_path

6.14 Reeds Shepp路径规划

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

相关阅读:

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

6.15 基于LQR的路径规划

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

6.16 Frenet Frame中的最优路径

这段代码在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

七、路径跟踪

7.1 姿势控制跟踪

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

相关阅读:

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

7.2 纯追迹跟踪

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

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

相关阅读:

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

https://arxiv.org/abs/1604.07446

7.3 史坦利控制

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

相关阅读:

史坦利:赢得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

7.4 后轮反馈控制

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

相关阅读:

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

https://arxiv.org/abs/1604.07446

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

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

相关阅读:

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

https://github.com/ApolloAuto/apollo

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

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

相关阅读:

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

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

7.7 模型预测速度和转向控制

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

这段代码使用了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必备50种算法(附解析)相关推荐

  1. 收藏 | 一文洞悉Python必备50种算法(附解析)

    来源:CSDN 本文约3800字,建议阅读10分钟. 本文整理了实践中广泛应用的算法,简单语句诠释每个算法的基本思想. 本文是一些机器人算法(特别是自动导航算法)的Python代码合集. 其主要特点有 ...

  2. python50种算法_收藏 | 一文洞悉Python必备50种算法(附解析)

    本文是一些机器人算法(特别是自动导航算法)的Python代码合集. 其主要特点有以下三点:选择了在实践中广泛应用的算法:依赖最少:容易阅读,容易理解每个算法的基本思想.希望阅读本文后能对你有所帮助. ...

  3. python50种算法_一文洞悉Python必备50种算法

    本文是一些机器人算法(特别是自动导航算法)的Python代码合集. 其主要特点有以下三点:选择了在实践中广泛应用的算法:依赖最少:容易阅读,容易理解每个算法的基本思想.希望阅读本文后能对你有所帮助. ...

  4. python五大算法_一文洞悉Python必备50种算法!资深大牛至少得掌握25种!

    一.环境需求 二.怎样使用 三.本地化 3.1扩展卡尔曼滤波本地化 3.2无损卡尔曼滤波本地化 3.3粒子滤波本地化 3.4直方图滤波本地化 四.映射 4.1高斯网格映射 4.2光线投射网格映射 4. ...

  5. python50种算法_一文洞悉Python必备50种算法(附解析)!

    本文是一些机器人算法(特别是自动导航算法)的Python代码合集. 其主要特点有以下三点:选择了在实践中广泛应用的算法:依赖最少:容易阅读,容易理解每个算法的基本思想.希望阅读本文后能对你有所帮助. ...

  6. 在.Net framework下遍历XML文挡树的两种算法

    在阅读ASP.NET_XML深入编程技术 (PDF格式)一书的时候,发现遍历树的两种算法:深度优先和广度优先遍历文挡树,前一种需要使用递归,后者则不需要,本人大学时期数据结构学的不好,每每涉及到树,总 ...

  7. Python:实现graham scan葛立恒扫描法算法(附完整源码)

    Python:实现graham scan葛立恒扫描法算法 from __future__ import annotations from collections import deque from e ...

  8. python:实现DES和3DES加解密算法(附完整源码)

    python:实现DES和3DES加解密算法 import sys _pythonMajorVersion = sys.version_info[0]# Modes of crypting / cyp ...

  9. XML文档定义有几种形式?解析XML文档有哪几种方式?

    XML文档定义有几种形式?它们之间有何本质区别?解析XML文档有哪几种方式? a: 两种形式 dtd schema,b: 本质区别:schema本身是xml的,可以被XML解析器解析(这也是从DTD上 ...

最新文章

  1. pandas使用rename函数重命名dataframe中数据列的名称、从而创建一个包含重复列名称的dataframe数据集
  2. Linker Tools Error LNK2001 报错
  3. android 线性布局 底部,java – 如何在android线性布局中对齐父底部?
  4. 原因以及如何避免产生僵尸进程
  5. 成本管理4个过程及相关重点
  6. 使用Java代码连接SAP ABAP Netweaver服务器
  7. 微信红包随机数字_微信红包随机算法转载
  8. 《精通.Net核心技术》
  9. (二十三)图像相似度比较哈希算法
  10. N720实现4G的TCP连接,数据上云的方法
  11. 一共81个,开源大数据处理工具汇总(上)
  12. python基础教程第三版pdf 脚本之家-一篇不错的Python入门教程
  13. 猫哥教你写爬虫 040--存储数据-作业
  14. unity17——布料插件制作旗子飘动效果
  15. java 取整符号_浅析java中的取整(/)和求余(%)
  16. 三维高清vR展示模型的应用及优势
  17. 美团-移动端UI一致性解决方案
  18. 使用U盘在虚拟机下安装双系统(windows and linux)乃至多系统
  19. 泰山OFFICE技术讲座:FreeType获取kerning函数FT_Get_Kerning研究
  20. python---引用其他py文件中的函数

热门文章

  1. 一周热图|杨幂春季时尚大片出炉;宋轶演绎人文地理主题商务时装;陈伟霆任健安喜大中华区品牌代言人...
  2. 程序员 颈椎健康不容忽视
  3. linux qt compiler叹号,qt里感叹号什么意思
  4. OSPF常见故障排除
  5. oracle主键约束删除,oracle删除主键查看主键约束及创建联合主键
  6. 爱错过了,就不在了!
  7. PRM算法三维地图matlab仿真
  8. 赵本山2007春晚最新小品曝光
  9. 中国vSAN团队在上海招聘Sales和SE
  10. 高并发场景下backlog详解