这可能是史上最全的 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星算法进行基于二维网格的最短路径规划。

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

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

势场算法

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

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