PythonRobotics 入门指南：开源机器人算法示例代码库的安装、模块导览与源码结构解析

导读

PythonRobotics 是一个以 Python 示例代码 + 配套教科书（textbook） 形式组织的机器人算法开源仓库，覆盖定位（Localization）、建图（Mapping）、SLAM、路径规划（Path Planning）、路径跟踪（Path Tracking）、机械臂导航（Arm Navigation）、空中导航（Aerial Navigation）与双足规划（Bipedal）等经典方向。本文以仓库根目录的 README.md 为骨架，结合 requirements/、tests/、utils/ 等目录的实际源码，为你梳理：如何搭建运行环境、如何运行每一个示例脚本、每个模块的核心算法与实现位置，以及仓库自带的测试与文档构建体系。读完本文，你将能够独立拉取代码、安装依赖、逐个运行示例，并顺着源码路径深入理解每个算法的工程实现。

---

一、项目概览：这是什么，解决什么问题

README.md 开篇即明确了项目的定位：Python codes and textbook for robotics algorithm——它既是一套可直接运行的机器人算法 Python 代码集合，又是一本配套的算法教科书（在线文档托管在 docs/ 目录，由 Sphinx 构建）。

项目在设计上有三个核心诉求：

1. 易读性（Easy to read）：每个示例代码的首要目标是帮助读者理解对应算法的基本思想，因此代码风格偏向教学化、单文件化，一个示例通常就是一个可独立运行的 .py 脚本；
2. 实用性与通用性（Widely used and practical）：入选的算法均为机器人领域被广泛使用、具备工程价值的经典算法（如 EKF、粒子滤波、A*、RRT、Stanley 控制等）；
3. 最小依赖（Minimum dependency）：运行示例仅依赖少量科学计算库，尽量不引入重型第三方框架。

该项目的学术背景可追溯至论文 "PythonRobotics: a Python code collection of robotics algorithms"（arXiv:1808.10703），README 亦建议在学术工作中引用该论文。

仓库的目录结构本身就是一张“算法地图”，顶层按应用领域划分：

顶层目录	覆盖内容
Localization	EKF、粒子滤波、直方图滤波等定位算法
Mapping	高斯栅格图、射线投影栅格图、点云聚类、矩形拟合等
SLAM	ICP 匹配、FastSLAM 1.0、图优化 SLAM 等
PathPlanning	A*、Dijkstra、RRT 系列、PRM、样条路径、Frenet 轨迹等
PathTracking	Stanley、纯追踪、后轮反馈、LQR、MPC、C-GMRES 等
ArmNavigation	N 关节机械臂控制、避障导航
AerialNavigation	四旋翼 3D 轨迹跟踪、火箭动力着陆
Bipedal	基于倒立摆的双足步态规划
utils	共享的角度、绘图工具函数

---

二、环境要求与安装步骤

2.1 运行示例所需依赖

根据 README.md 的 "Requirements to run the code" 一节，运行每个示例脚本需要：

• Python 3.13.x
• NumPy：数值计算基础库
• SciPy：科学计算库（用于优化、线性代数等）
• Matplotlib：可视化绘图（所有示例的动画与图表输出）
• cvxpy：凸优化建模库（主要用于 MPC、轨迹优化类示例）

仓库同时提供了精确锁版本的依赖清单 requirements/requirements.txt：

numpy == 2.3.5
scipy == 1.18.1
matplotlib == 3.11.0
cvxpy == 1.8.1
ecos == 2.0.14
pytest == 9.1.0 # For unit test
pytest-xdist == 3.8.0 # For unit test
mypy == 1.19.1 # For unit test
ruff == 0.16.5 # For unit test

注意：requirements.txt 中除运行依赖外，还一并锁定了测试与静态检查工具（pytest、pytest-xdist、mypy、ruff）的版本。

2.2 开发（测试与文档）所需依赖

若你不仅想运行示例，还希望参与开发、跑测试或构建文档，README.md 列出了以下工具：

• pytest：单元测试框架；
• pytest-xdist：并行运行单元测试；
• mypy：静态类型检查（仓库根目录有 mypy.ini 配置）；
• sphinx：文档生成（配合 docs/conf.py）；
• ruff / pycodestyle：代码风格检查（仓库使用 ruff.toml）。

2.3 三种安装方式

README.md 的 "How to use" 一节给出了标准流程，首先克隆仓库：

git clone https://gitcode.com/GitHub_Trending/py/PythonRobotics.git

然后安装依赖，有两种等价方式：

方式一：conda（推荐，使用 conda-forge 频道）

conda env create -f requirements/environment.yml

对应的 requirements/environment.yml 内容如下，环境名为 python_robotics：

name: python_robotics
channels:
  - conda-forge
dependencies:
  - python=3.13
  - pip
  - scipy
  - numpy
  - cvxpy
  - matplotlib

方式二：pip

pip install -r requirements/requirements.txt

安装完成后，直接进入对应算法的子目录执行该目录下的 Python 脚本即可看到仿真动画，例如：

python PathPlanning/AStar/a_star.py
python Localization/extended_kalman_filter/extended_kalman_filter.py

这是 README 给出的最简运行模式：每个示例都是自包含脚本，无需注册、无需额外配置。多数示例在文件顶部提供 show_animation = True 开关，置为 False 即可在无图形界面环境下静默运行（详见下文测试一节）。

---

三、配套文档（Textbook）与本地构建

README 明确指出：它只展示了项目的一部分示例。如果你需要更多示例、每个算法的数学背景推导，应查阅完整的在线教科书文档。在仓库内，这份教科书的源文件位于 docs 目录，采用 Sphinx + reStructuredText 组织，章节与代码模块一一对应，例如：

• docs/modules/2_localization：定位章节；
• docs/modules/5_path_planning：路径规划章节；
• docs/modules/6_path_tracking：路径跟踪章节。

本地构建文档的方法记录在 docs/README.md 中：

# 1. 安装 Sphinx 及相关主题
pip install sphinx sphinx-autobuild sphinx-rtd-theme sphinx_rtd_dark_mode sphinx_copybutton

# 2. 在 docs/ 目录下构建
make html

# 3. 若希望在文件变更时自动重建
sphinx-autobuild . _build/html

构建完成后，打开 docs/_build/html 下的 index.html 即可浏览生成的教科书。README 还提到，所有示例的动画 GIF 资源独立存放在单独的动画资源仓库中（AtsushiSakai/PythonRoboticsGifs），在线文档内嵌了这些动画以直观展示算法运行效果。

---

四、模块逐项导览（含源码定位）

README 的主体部分按“定位 → 建图 → SLAM → 路径规划 → 路径跟踪 → 机械臂 → 空中导航 → 双足”的顺序逐一介绍各示例。下面沿此脉络展开，并为每个示例标注仓库内的源码路径，方便你对照阅读。

4.1 Localization 定位

Extended Kalman Filter localization（EKF 定位）

• 源码：Localization/extended_kalman_filter/extended_kalman_filter.py
• 配套文档源文件：docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst

这是一个标准的 EKF 定位仿真：机器人通过运动模型递推位姿（预测），并用 GPS 观测修正（更新）。从源码可以读出关键仿真参数（extended_kalman_filter.py）：

# Covariance for EKF simulation
Q = np.diag([
    0.1,                # variance of location on x-axis
    0.1,                # variance of location on y-axis
    np.deg2rad(1.0),    # variance of yaw angle
    1.0                 # variance of velocity
]) ** 2                 # predict state covariance
R = np.diag([1.0, 1.0]) ** 2   # Observation x,y position covariance

# Simulation parameter
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
GPS_NOISE = np.diag([0.5, 0.5]) ** 2

DT = 0.1        # time tick [s]
SIM_TIME = 50.0 # simulation time [s]

其中 Q 为预测过程噪声协方差（状态为 [x, y, yaw, v]），R 为观测噪声协方差，INPUT_NOISE 与 GPS_NOISE 分别模拟输入与 GPS 观测的不确定性，DT 为仿真步长、SIM_TIME 为总仿真时长。结果绘制中使用的误差椭圆由共享工具函数 plot_covariance_ellipse 完成，位于 utils/plot.py，它通过对协方差矩阵做特征值分解得到椭圆的长短轴与朝向，chi2=3.0 对应约 95% 置信区间（2D 高斯分布）。

Particle filter localization（粒子滤波定位）

• 源码：Localization/particle_filter/particle_filter.py

这是一个粒子滤波传感器融合定位示例：图中蓝色线为真实轨迹，黑色线为航位推算（dead reckoning）轨迹，红色线为 PF 估计轨迹。假设机器人可以测量到与地标（RFID）之间的距离，这些观测被用于 PF 定位的权重更新。示例引用的算法背景是经典教材 Probabilistic Robotics。

Histogram filter localization（直方图滤波定位）

• 源码：Localization/histogram_filter/histogram_filter.py

这是一个 2D 直方图滤波定位示例：红色十字为真实位置，黑色点为 RFID 位置，蓝色栅格表示直方图滤波给出的位置概率分布。仿真中 x、y 未知而 yaw 已知，滤波器融合速度输入与 RFID 距离观测进行定位，无需初始位置假设（这是直方图/网格方法相对卡尔曼滤波族的重要特点）。

4.2 Mapping 建图

• Gaussian grid map（高斯栅格图）：Mapping/gaussian_grid_map/gaussian_grid_map.py，2D 高斯栅格建图示例，每个栅格维护高斯概率分布。
• Ray casting grid map（射线投影栅格图）：Mapping/ray_casting_grid_map/ray_casting_grid_map.py，2D 射线投影栅格建图。
• Lidar to grid map：Mapping/lidar_to_grid_map/lidar_to_grid_map.py，演示如何将 2D 距离测量（激光雷达）转换为栅格地图。
• k-means object clustering：源码 Mapping/kmeans_clustering/kmeans_clustering.py，配套文档 docs/modules/3_mapping/k_means_object_clustering，2D 物体聚类。
• Rectangle fitting（矩形拟合）：源码 Mapping/rectangle_fitting/rectangle_fitting.py 及仿真器 Mapping/rectangle_fitting/simulator.py，用于车辆检测场景的 2D 矩形拟合。

此外 Mapping 目录还包含 README 未展开、但属于同一领域的扩展实现，如 DistanceMap/distance_map.py、circle_fitting/circle_fitting.py、ndt_map/ndt_map.py、normal_vector_estimation、point_cloud_sampling 等，可在在线教科书的建图章节中找到对应讲解。

4.3 SLAM 同步定位与建图

Iterative Closest Point (ICP) Matching

• 源码：SLAM/ICPMatching/icp_matching.py
• 配套文档：docs/modules/4_slam/iterative_closest_point_matching

这是基于**奇异值分解（SVD）**的 2D ICP 匹配示例，用于计算两组点云之间的旋转矩阵与平移向量。

FastSLAM 1.0

• 源码：SLAM/FastSLAM1/fast_slam1.py
• 配套文档：docs/modules/4_slam/FastSLAM1

基于特征的 FastSLAM 1.0 示例：蓝色线为真实轨迹（ground truth），黑色线为航位推算，红色线为 FastSLAM 估计轨迹；红色点为粒子，黑色点为真实地标，蓝色十字为 FastSLAM 估计的地标位置。

SLAM 目录还提供 README 未列举的扩展，包括 EKFSLAM/ekf_slam.py、FastSLAM2/fast_slam2.py 以及图优化 SLAM GraphBasedSLAM/graph_based_slam.py（含 SE2 位姿图实现，见 GraphBasedSLAM/graphslam）。

4.4 Path Planning 路径规划

这是 README 中内容最丰富的章节，按算法家族组织：

Dynamic Window Approach（动态窗口法）

• 源码：PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
• 配套文档：docs/modules/5_path_planning/dynamic_window_approach

2D 动态窗口法避障导航示例，算法原始出处为 The Dynamic Window Approach to Collision Avoidance（Fox, Dieter 等，1997）。

Grid based search（栅格搜索）

• Dijkstra：PathPlanning/Dijkstra/dijkstra.py，动画中青色点为已搜索节点；
• A*：PathPlanning/AStar/a_star.py，启发式函数为 2D 欧氏距离，青色点为已搜索节点；
• D*：PathPlanning/DStar/dstar.py，展示机器人用 D* 搜索避障寻路；
• D* Lite：PathPlanning/DStarLite/d_star_lite.py，在 2D 栅格上实现：发现新障碍时重规划最短路径；
• Potential Field：PathPlanning/PotentialFieldPlanning/potential_field_planning.py，蓝色热力图显示每个栅格上的势场值；
• Grid based coverage path planning：PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py，2D 全覆盖路径规划仿真（同目录还有 WavefrontCPP、SpiralSpanningTreeCPP 等覆盖规划实现）；
• Particle Swarm Optimization (PSO)：PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py，受鸟群觅食启发的元启发式优化：粒子（蓝色点）在搜索空间中探索，从起点（绿色区域）向目标（红色星号）收敛出无碰撞路径（黄色线）。

以 A* 为例，源码 a_star.py 中的 AStarPlanner 类清晰展示了栅格化流程：构造函数接收障碍物坐标列表 ox/oy、栅格分辨率 resolution 与机器人半径 rr，据此建立障碍物栅格地图 calc_obstacle_map，并通过 get_motion_model() 生成 8 邻域运动代价模型。配套测试 tests/test_a_star.py 通过把 show_animation 置为 False 后调用 main() 来验证算法可无界面运行。

State Lattice Planning（状态格规划）

• 源码：PathPlanning/StateLatticePlanner/state_lattice_planner.py
• 配套实现：PathPlanning/ModelPredictiveTrajectoryGenerator（含 motion_model.py、trajectory_generator.py、lookup_table_generator.py）

该脚本使用**模型预测轨迹生成器（Model Predictive Trajectory Generator）**求解边界值问题。README 给出两种采样模式：

• Biased polar sampling（偏置极坐标采样）；
• Lane sampling（车道采样）。

配套文档见 docs/modules/5_path_planning/state_lattice_planner，参考论文包括 Optimal rough terrain trajectory generation for wheeled mobile robots 与 State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments。

Probabilistic Road-Map (PRM) planning（概率路图）

• 源码：PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py

PRM 规划器使用 Dijkstra 方法做图搜索：蓝色点为采样点，青色十字为 Dijkstra 搜索过的点，红色线为最终路径。

Rapidly-Exploring Random Trees (RRT) 系列

• RRT*：PathPlanning/RRTStar/rrt_star.py，黑色圆为障碍物、绿色线为搜索树、红色十字为起点与终点；基础版 RRT 见 PathPlanning/RRT/rrt.py（含路径平滑与 Sobol 采样变体）。
• RRT* with Reeds-Shepp path：PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py，面向汽车机器人（满足非完整约束）的 RRT* + Reeds-Shepp 路径规划。
• LQR-RRT*：PathPlanning/LQRRRTStar/lqr_rrt_star.py，使用双积分器运动模型作为 LQR 局部规划器。

其他路径生成类算法

• Quintic polynomials planning：PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py，基于五次多项式计算 2D 路径、速度与加速度剖面；
• Reeds Shepp planning：PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py，面向可前进/后退车辆的 Reeds-Shepp 曲线；
• LQR based path planning：PathPlanning/LQRPlanner/lqr_planner.py，基于 LQR 的路径规划（双积分器模型）；
• Optimal Trajectory in a Frenet Frame：PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py，Frenet 坐标系下的最优轨迹生成：青色线为目标路线、黑色十字为障碍物、红色线为预测路径，配套实现 cartesian_frenet_converter.py，参考论文为 Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame。

4.5 Path Tracking 路径跟踪

• move to a pose control：PathTracking/move_to_pose/move_to_pose.py，运动到位姿控制仿真，参考 Robotics, Vision and Control（P. I. Corke）；
• Stanley control：PathTracking/stanley_control/stanley_control.py，Stanley 转向控制 + PID 速度控制的路径跟踪仿真，参考 DARPA 挑战赛冠军车辆 Stanley 论文；
• Rear wheel feedback control：PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py，后轮反馈转向 + PID 速度控制；
• LQR speed and steering control：PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py，LQR 速度与转向联合控制；
• Model predictive speed and steering control：PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py，迭代线性模型预测的速度与转向控制；
• Nonlinear MPC with C-GMRES：PathTracking/cgmres_nmpc/cgmres_nmpc.py，基于 C-GMRES 的非线性 MPC 运动规划与路径跟踪仿真，配套文档 docs/modules/6_path_tracking/cgmres_nmpc。

4.6 Arm Navigation 机械臂导航

• N joint arm to point control：ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py，交互式仿真：在绘图区左键点击即可设置末端执行器目标位置。默认 N = 10 关节，可自行修改；核心模型见 ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py；
• Arm navigation with obstacle avoidance：ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py，机械臂避障导航仿真；同目录还提供 3D 版本 arm_obstacle_navigation_2.py 与随机正/逆运动学示例 ArmNavigation/n_joint_arm_3d。

4.7 Aerial Navigation 空中导航

• drone 3d trajectory following：AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py，四旋翼（quadrotor）3D 轨迹跟踪仿真，模型见同目录 Quadrotor.py、轨迹生成见 TrajectoryGenerator.py；
• rocket powered landing：AerialNavigation/rocket_powered_landing/rocket_powered_landing.py，火箭动力着陆的 3D 轨迹生成仿真，配套文档 docs/modules/8_aerial_navigation/rocket_powered_landing。

4.8 Bipedal 双足

• bipedal planner with inverted pendulum：Bipedal/bipedal_planner/bipedal_planner.py，基于倒立摆模型的双足步态规划器：你可以自行设置落脚点，规划器会自动修正这些落脚点以维持倒立摆平衡，配套文档 docs/modules/9_bipedal/bipedal_planner。

---

五、测试体系：如何验证与运行全部示例

仓库在 tests 目录下为几乎每个示例都配备了单元测试，命名与源码一一对应（如 test_a_star.py 对应 PathPlanning/AStar，test_extended_kalman_filter.py 对应 EKF）。其核心套路可从 tests/conftest.py 与 tests/test_a_star.py 看出：

1. conftest.py 通过 sys.path.append 把仓库根目录与测试目录注入 Python 路径，使测试可以直接 from PathPlanning.AStar import a_star as m 导入各模块；
2. 测试统一将模块级开关 show_animation 置为 False，随后调用该模块的 main() 或核心函数，从而在无图形界面环境（CI）下完整执行一遍算法逻辑；
3. 测试基于 pytest 编写，运行方式见 runtests.sh：

pytest tests -l -Werror --durations=0

其中 -Werror 将警告视为错误、--durations=0 输出各测试耗时排名、-l 在断言失败时显示局部变量，便于调试。若安装了 pytest-xdist，还可以并行加速。

这一测试设计也给了读者一个实用技巧：想无头（headless）运行任何示例，只需把脚本顶部的 show_animation 改为 False，这正是所有测试用例采用的运行方式。

---

六、许可证、使用案例与贡献

• License：README 明确项目采用 MIT 许可证（详见仓库根目录 LICENSE），代码可自由用于学习、研究与商业项目。
• Use-case：项目维护者欢迎用户反馈使用案例，用户评价与参考列表集中记录在 users_comments.md。若你的机器人项目受益于此仓库，可创建 issue 分享视频或说明。
• Contribution：任何形式的贡献都受欢迎，贡献指南位于 CONTRIBUTING.md，在线文档亦提供 "How To Contribute" 章节（源文件见 docs/modules/0_getting_started/3_how_to_contribute_main.rst）。
• Citing：若在学术工作中使用本仓库代码，README 建议引用论文 PythonRobotics: a Python code collection of robotics algorithms（arXiv:1808.10703）。
• 社区与安全：仓库同时包含 CODE_OF_CONDUCT.md 与 SECURITY.md，以及持续集成配置（如 appveyor.yml）。

---

七、小结与推荐学习路径

综合 README 与源码可以总结出三条递进的学习路径：

1. 快速上手：克隆仓库 → 按 requirements/ 安装依赖 → 逐个运行各子目录脚本观察动画，对照 README 中每个算法的图形说明理解输出含义；
2. 深入原理：对感兴趣的算法，阅读其单文件源码（如 EKF 的 extended_kalman_filter.py、A* 的 a_star.py），结合 docs 下的对应章节了解数学背景；
3. 工程化验证：运行 runtests.sh 对应的 pytest 测试套件，参考 tests 中的用例理解每个模块的输入输出契约，进而修改参数（如 EKF 中的噪声协方差、A* 中的栅格分辨率）观察算法行为变化。

对初学者而言，建议从 PathPlanning/AStar（最短路径）、PathTracking/stanley_control（路径跟踪）与 Localization/extended_kalman_filter（状态估计）这三个经典示例入手——它们覆盖了机器人学“感知—规划—控制”的核心闭环，且代码量小、可读性强，与项目“易读、实用、最小依赖”的设计初衷完全一致。