# PythonRobotics
**Repository Path**: aichizhe/python-robotics
## Basic Information
- **Project Name**: PythonRobotics
- **Description**: No description available
- **Primary Language**: Python
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No
## Statistics
- **Stars**: 1
- **Forks**: 1
- **Created**: 2025-12-19
- **Last Updated**: 2025-12-19
## Categories & Tags
**Categories**: Uncategorized
**Tags**: None
## README
# PythonRobotics
[](https://ci.appveyor.com/project/AtsushiSakai/pythonrobotics)
用于机器人算法的Python代码和[教科书](https://atsushisakai.github.io/PythonRobotics/index.html)。
# 目录
* [这是什么？](#什么是-pythonrobotics)
* [要求](#运行代码的要求)
* [文档](#文档教科书)
* [如何使用](#如何使用)
* [定位](#定位)
* [扩展卡尔曼滤波定位](#扩展卡尔曼滤波定位)
* [粒子滤波定位](#粒子滤波定位)
* [直方图滤波定位](#直方图滤波定位)
* [建图](#建图)
* [高斯栅格地图](#高斯栅格地图)
* [射线投射栅格地图](#射线投射栅格地图)
* [激光雷达到栅格地图](#激光雷达到栅格地图)
* [k-means物体聚类](#k-means物体聚类)
* [矩形拟合](#矩形拟合)
* [SLAM](#slam)
* [迭代最近点(ICP)匹配](#迭代最近点icp匹配)
* [FastSLAM 1.0](#fastslam-10)
* [路径规划](#路径规划)
* [动态窗口法](#动态窗口法)
* [基于网格的搜索](#基于网格的搜索)
* [Dijkstra算法](#dijkstra算法)
* [A*算法](#a算法)
* [D*算法](#d算法)
* [D* Lite算法](#d-lite算法)
* [势场算法](#势场算法)
* [基于网格的覆盖路径规划](#基于网格的覆盖路径规划)
* [粒子群优化(PSO)](#粒子群优化pso)
* [状态格规划](#状态格规划)
* [偏向极坐标采样](#偏向极坐标采样)
* [车道采样](#车道采样)
* [概率路线图(PRM)规划](#概率路线图prm规划)
* [快速探索随机树(RRT)](#快速探索随机树rrt)
* [RRT*](#rrt)
* [带Reeds-Shepp路径的RRT*](#带reeds-shepp路径的rrt)
* [LQR-RRT*](#lqr-rrt)
* [五次多项式规划](#五次多项式规划)
* [Reeds Shepp规划](#reeds-shepp规划)
* [基于LQR的路径规划](#基于lqr的路径规划)
* [Frenet框架中的最优轨迹](#frenet框架中的最优轨迹)
* [路径跟踪](#路径跟踪)
* [移动到姿态控制](#移动到姿态控制)
* [Stanley控制](#stanley控制)
* [后轮反馈控制](#后轮反馈控制)
* [线性二次调节器(LQR)速度和转向控制](#线性二次调节器lqr速度和转向控制)
* [模型预测速度和转向控制](#模型预测速度和转向控制)
* [使用C-GMRES的非线性模型预测控制](#使用c-gmres的非线性模型预测控制)
* [机械臂导航](#机械臂导航)
* [N关节臂到点控制](#n关节臂到点控制)
* [带障碍物规避的机械臂导航](#带障碍物规避的机械臂导航)
* [空中导航](#空中导航)
* [无人机3D轨迹跟随](#无人机3d轨迹跟随)
* [火箭动力着陆](#火箭动力着陆)
* [双足机器人](#双足机器人)
* [倒立摆双足规划器](#倒立摆双足规划器)
* [许可证](#许可证)
* [使用案例](#使用案例)
* [贡献](#贡献)
* [引用](#引用)
* [支持](#支持)
* [赞助商](#赞助商)
* [JetBrains](#jetbrains)
* [1Password](#1password)
* [作者](#作者)
# 什么是 PythonRobotics?
PythonRobotics 是一个Python代码集合和机器人算法的[教科书](https://atsushisakai.github.io/PythonRobotics/index.html)。
特点：
1. 易于阅读，便于理解每个算法的基本思想。
2. 选择广泛使用且实用的算法。
3. 最小依赖。
查看此文档
- [入门 — PythonRobotics 文档](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/1_what_is_python_robotics.html)
或者这个YouTube视频：
- [PythonRobotics 项目音频概述](https://www.youtube.com/watch?v=uMeRnNoJAfU)
或者这篇论文了解更多详情：
- [[1808.10703] PythonRobotics：机器人算法的Python代码集合](https://arxiv.org/abs/1808.10703) ([BibTeX](https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib))
# 运行代码的要求
运行每个示例代码的要求：
- [Python 3.13.x](https://www.python.org/)
- [NumPy](https://numpy.org/)
- [SciPy](https://scipy.org/)
- [Matplotlib](https://matplotlib.org/)
- [cvxpy](https://www.cvxpy.org/)
开发要求：
- [pytest](https://pytest.org/) (用于单元测试)
- [pytest-xdist](https://pypi.org/project/pytest-xdist/) (用于并行单元测试)
- [mypy](https://mypy-lang.org/) (用于类型检查)
- [sphinx](https://www.sphinx-doc.org/) (用于文档生成)
- [pycodestyle](https://pypi.org/project/pycodestyle/) (用于代码风格检查)
# 文档 (教科书)
此README仅显示该项目的一些示例。
如果您对其他示例或每个算法的数学背景感兴趣，
您可以在线查看完整文档（教科书）：[欢迎来到PythonRobotics的文档！— PythonRobotics 文档](https://atsushisakai.github.io/PythonRobotics/index.html)
所有动画gif都存储在这里：[AtsushiSakai/PythonRoboticsGifs: PythonRobotics 的动画 gif](https://github.com/AtsushiSakai/PythonRoboticsGifs)
# 如何使用
1. 克隆此仓库。
```terminal
git clone https://github.com/AtsushiSakai/PythonRobotics.git
```
2. 安装所需的库。
- 使用 conda :
```terminal
conda env create -f requirements/environment.yml
```
- 使用 pip :
```terminal
pip install -r requirements/requirements.txt
```
3. 执行每个目录中的python脚本。
4. 如果您喜欢这个项目，请给它加星标 :smiley:。
# 定位
## 扩展卡尔曼滤波定位
参考
- [文档](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
## 粒子滤波定位
这是一个使用粒子滤波(PF)的传感器融合定位。
蓝线是真实轨迹，黑线是航位推算轨迹，
红线是使用PF估计的轨迹。
假设机器人可以测量来自地标(RFID)的距离。
这些测量值用于PF定位。
参考
- [概率机器人](http://www.probabilistic-robotics.org/)
## 直方图滤波定位
这是一个使用直方图滤波的2D定位示例。
红十字是真实位置，黑点是RFID位置。
蓝色网格显示了直方图滤波的位置概率。
在此模拟中，x,y未知，偏航角已知。
滤波器集成了速度输入和来自RFID的距离观测进行定位。
不需要初始位置。
参考
- [概率机器人](http://www.probabilistic-robotics.org/)
# 建图
## 高斯栅格地图
这是一个2D高斯栅格地图示例。
## 射线投射栅格地图
这是一个2D射线投射栅格地图示例。
## 激光雷达到栅格地图
此示例展示了如何将2D距离测量转换为栅格地图。
## k-means 物体聚类
这是一个使用k-means算法的2D物体聚类。
## 矩形拟合
这是一个用于车辆检测的2D矩形拟合。
# SLAM
同时定位与建图(SLAM)示例
## 迭代最近点(ICP)匹配
这是一个使用奇异值分解的2D ICP匹配示例。
它可以计算点与点之间的旋转矩阵和平移向量。
参考
- [移动机器人导论：迭代最近点算法](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)
## FastSLAM 1.0
这是一个使用FastSLAM 1.0的基于特征的SLAM示例。
蓝线是真实情况，黑线是航位推算，红线是使用FastSLAM估计的轨迹。
红点是FastSLAM的粒子。
黑点是地标，蓝叉是FastSLAM估计的地标位置。
参考
- [概率机器人](http://www.probabilistic-robotics.org/)
- [Tim Bailey的SLAM模拟](http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm)
# 路径规划
## 动态窗口法
这是一个使用动态窗口法的2D导航示例代码。
- [动态窗口法避障](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf)
## 基于网格的搜索
### Dijkstra算法
这是一个使用Dijkstra算法的2D网格最短路径规划。
在动画中，青色点是被搜索的节点。
### A*算法
这是一个使用A*算法的2D网格最短路径规划。
在动画中，青色点是被搜索的节点。
其启发式函数是2D欧几里得距离。
### D*算法
这是一个使用D*算法的2D网格最短路径规划。
动画显示了一个机器人使用D*搜索算法避开障碍物找到路径。
参考
- [D*算法维基百科](https://en.wikipedia.org/wiki/D*)
### D* Lite算法
该算法可以在发现障碍物时重新规划两点间的最短路径。这里针对2D网格进行了实现。
动画显示了一个机器人在寻找路径并在发现障碍物时重新规划路径的过程中使用D* Lite搜索算法。
参考文献：
- [D* Lite](http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf)
- [改进的未知地形机器人快速重规划](http://www.cs.cmu.edu/~maxim/files/dlite_icra02.pdf)
### 势场算法
这是一个使用势场算法的2D网格路径规划。
在动画中，蓝色热图显示了每个网格上的势值。
参考
- [机器人运动规划：势函数](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf)
### 基于网格的覆盖路径规划
这是一个2D网格覆盖路径规划模拟。
### 粒子群优化(PSO)
这是一个使用粒子群优化算法的2D路径规划模拟。
PSO是一种受鸟类群体行为启发的元启发式优化算法。在路径规划中，粒子探索搜索空间以寻找无碰撞路径同时避开障碍物。
动画显示了粒子（蓝点）从起点（绿色区域）到目标点（红星）收敛到最优路径（黄线）的过程。
参考文献
- [粒子群优化 - 维基百科](https://en.wikipedia.org/wiki/Particle_swarm_optimization)
- [Kennedy, J.; Eberhart, R. (1995). "粒子群优化"](https://ieeexplore.ieee.org/document/488968)
## 状态格规划
此脚本是一个使用状态格规划的路径规划代码。
此代码使用模型预测轨迹生成器来解决边界问题。
参考
- [轮式移动机器人的最优粗糙地形轨迹生成](https://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
- [复杂环境中高机动性移动机器人导航的状态空间采样](https://www.cs.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
### 偏向极坐标采样
### 车道采样
## 概率路线图(PRM)规划
此PRM规划器使用Dijkstra方法进行图搜索。
在动画中，蓝点是采样点，
青色叉表示使用Dijkstra方法搜索的点，
红线是PRM的最终路径。
参考
- [概率路线图 - 维基百科](https://en.wikipedia.org/wiki/Probabilistic_roadmap)
## 快速探索随机树(RRT)
### RRT*
这是一个使用RRT*的路径规划代码
黑圈是障碍物，绿线是搜索树，红叉是起始和目标位置。
参考
- [最优运动规划的增量采样算法](https://arxiv.org/abs/1005.0416)
- [最优运动规划的采样算法](https://citeseerx.ist.psu.edu/document?repid=rep1&type=pdf&doi=bddbc99f97173430aa49a0ada53ab5bade5902fa)
### 带Reeds-Shepp路径的RRT*
使用RRT*和Reeds-Shepp路径规划器为汽车机器人进行路径规划。
### LQR-RRT*
这是一个使用LQR-RRT*的路径规划模拟。
使用双积分器运动模型作为LQR局部规划器。
参考
- [LQR-RRT*：具有自动推导扩展启发式的最优采样运动规划](https://lis.csail.mit.edu/pubs/perez-icra12.pdf)
- [MahanFathi/LQR-RRTstar：LQR-RRT*方法用于简单摆在其相图中的随机运动规划](https://github.com/MahanFathi/LQR-RRTstar)
## 五次多项式规划
使用五次多项式进行运动规划。
它可以基于五次多项式计算2D路径、速度和加速度轮廓。
参考
- [AGV定位中的局部路径规划和运动控制](https://ieeexplore.ieee.org/document/637936/)
## 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)
## 基于LQR的路径规划
使用LQR基于双积分器模型的路径规划示例代码。
## Frenet框架中的最优轨迹
这是在Frenet框架中的最优轨迹生成。
青色线是目标路线，黑色叉号是障碍物。
红线是预测路径。
参考
- [动态街道场景中Frenet框架下的最优轨迹生成](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
- [Frenet框架下动态街道场景的最优轨迹生成](https://www.youtube.com/watch?v=Cj6tAQe7UCY)
# 路径跟踪
## 移动到姿态控制
这是一个移动到姿态控制的模拟
参考
- [P. I. Corke, "机器人学、视觉与控制" | SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)
## Stanley 控制
使用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)
## 后轮反馈控制
使用后轮反馈转向控制和PID速度控制的路径跟踪模拟。
参考
- [自动驾驶城市车辆的运动规划与控制技术综述](https://arxiv.org/abs/1604.07446)
## 线性二次调节器(LQR)速度和转向控制
使用LQR速度和转向控制的路径跟踪模拟。
参考
- [迈向完全自主驾驶：系统与算法 - IEEE会议出版物](https://ieeexplore.ieee.org/document/5940562/)
## 模型预测速度和转向控制
使用迭代线性模型预测速度和转向控制的路径跟踪模拟。
参考
- [文档](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html)
- [实时模型预测控制(MPC)，ACADO，Python | Work-is-Playing](http://grauonline.de/wordpress/?page_id=3244)
## 使用C-GMRES的非线性模型预测控制
使用C-GMRES NMPC的运动规划和路径跟踪模拟
参考
- [文档](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/cgmres_nmpc/cgmres_nmpc.html)
# 机械臂导航
## N关节臂到点控制
N关节臂到点控制模拟。
这是一个交互式模拟。
您可以通过在绘图区域左键单击来设置末端执行器的目标位置。
在此模拟中N=10，但您可以更改它。
## 带障碍物规避的机械臂导航
带障碍物规避的机械臂导航模拟。
# 空中导航
## 无人机3D轨迹跟随
这是一个四旋翼无人机的3D轨迹跟随模拟。
## 火箭动力着陆
这是一个火箭动力着陆的3D轨迹生成模拟。
参考
- [文档](https://atsushisakai.github.io/PythonRobotics/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)
# 双足机器人
## 带倒立摆的双足规划器
这是一个修改倒立摆脚步的双足规划器。
您可以设置脚步，规划器会自动修改它们。
# 许可证
MIT
# 使用案例
如果此项目对您的机器人项目有帮助，请通过创建issue告知我们。
非常欢迎您使用PythonRobotics的机器人视频！！
这是用户评论和参考资料列表：[用户评论](https://github.com/AtsushiSakai/PythonRobotics/blob/master/users_comments.md)
# 贡献
欢迎任何贡献！！
请查看此文档：[如何贡献 — PythonRobotics 文档](https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/3_how_to_contribute.html)
# 引用
如果您在学术工作中使用此项目的代码，我们鼓励您引用[我们的论文](https://arxiv.org/abs/1808.10703)
如果您在工业界使用此项目的代码，我们也希望听到您的消息；请随时直接联系开发人员。
# 支持此项目
如果您或您的公司希望支持此项目，请考虑：
- [在GitHub Sponsors上赞助@AtsushiSakai](https://github.com/sponsors/AtsushiSakai)
- [在Patreon上成为支持者或赞助商](https://www.patreon.com/myenigma)
- [通过PayPal一次性捐赠](https://www.paypal.com/paypalme/myenigmapay/)
如果您想以其他方式支持我们，请通过创建issue与我们联系。
## 赞助商
### [JetBrains](https://www.jetbrains.com/)
他们为这个开源软件开发提供了免费的IDE许可证。
### [1Password](https://github.com/1Password/for-open-source)
他们为此开源项目提供了免费的1Password团队许可证。
# 作者
- [AtsushiSakai/PythonRobotics的贡献者](https://github.com/AtsushiSakai/PythonRobotics/graphs/contributors)