这可能是史上最全的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

怎样使用

安装必要的库;

克隆本代码仓库;

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

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

本地化

扩展卡尔曼滤波本地化

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

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

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

相关阅读:

概率机器人学

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

无损卡尔曼滤波本地化

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

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

相关阅读:

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

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

粒子滤波本地化

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

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

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

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

相关阅读:

概率机器人学

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

直方图滤波本地化

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

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

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

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

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

不需要初始位置。

相关阅读:

概率机器人学

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

映射

高斯网格映射

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

光线投射网格映射

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

k均值物体聚类

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

圆形拟合物体形状识别

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

蓝圈是实际的物体形状。

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

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

SLAM

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

迭代最近点匹配

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

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

相关阅读:

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

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

EKF SLAM

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

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

绿叉是估计的地标。

相关阅读:

概率机器人学

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

FastSLAM 1.0

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

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

红点是FastSLAM中的粒子。

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

相关阅读:

概率机器人学

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

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

基于图的SLAM

这是基于图的SLAM的示例。

蓝线是实际路径。

黑线是导航推测路径。

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

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

相关阅读:

基于图的SLAM入门

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

路径规划

动态窗口方式

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

相关阅读:

用动态窗口方式避免碰撞

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

基于网格的搜索

迪杰斯特拉算法

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

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

A*算法

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

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

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

势场算法

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

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

相关阅读:

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

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

模型预测路径生成

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

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

路径优化示例

查找表生成示例

  • 发表于:
  • 原文链接https://kuaibao.qq.com/s/20190220A0XZ9H00?refer=cp_1026
  • 腾讯「云+社区」是腾讯内容开放平台帐号(企鹅号)传播渠道之一,根据《腾讯内容开放平台服务协议》转载发布内容。

扫码关注云+社区

领取腾讯云代金券

年度创作总结 领取年终奖励