PythonRobotics 如何用 Reeds-Shepp 路径规划生成允许倒车的车辆最短路径【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics如果你的车可以在前进和倒车两个方向行驶比如泊车、窄路掉头只允许前进的 Dubins 路径往往不是最短的。PythonRobotics 的 Reeds-Shepp 路径规划模块就是为这个场景写的给定起点位姿x, y, yaw和终点位姿在最大曲率约束下从 CSC、CCC、CCCC、CCSC、CSCC、CCSCC 这几类 Reeds-Shepp 曲线中搜索并挑出总长度最短的一条负长度段表示需要倒着走。本文带你用仓库里的示例代码跑通一次规划看懂输出的路径模式与段长再用仓库自带的测试验证结果正确性。准备环境克隆仓库后按 README 的 How to use 安装依赖。两种方式任选其一# 方式一conda创建名为 python_robotics 的环境 conda env create -f requirements/environment.yml # 方式二pip pip install -r requirements/requirements.txtrequirements/environment.yml 固定了python3.13conda-forge 通道以及 scipy、numpy、cvxpy、matplotlib 四个依赖requirements/requirements.txt 给出对应的精确版本numpy 2.3.5、scipy 1.18.1、matplotlib 3.11.0 等。Reeds-Shepp 示例本身用到的是 numpy 和 matplotlib。运行 Reeds-Shepp 示例在仓库根目录下执行python PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.pyREADME 的使用说明就是“在对应目录下执行 python 脚本”该脚本自身会把仓库根目录加入sys.path因此在根目录用相对路径直接运行即可。脚本 reeds_shepp_path_planning.py 的main()使用以下默认参数单位均为米、弧度起点start_x -1.0、start_y -4.0、start_yaw np.deg2rad(-20.0)终点end_x 5.0、end_y 5.0、end_yaw np.deg2rad(25.0)最大曲率curvature 0.1插值步长step_size 0.05运行后脚本会打印选中路径的各段长度print(f{lengths})并用 matplotlib 画出最终路径图上曲线标签为final course str(modes)其中modes是路径模式序列起终点各画一个箭头表示初始与目标位姿。如果xs为空脚本会以assert False, No path终止这表示该起终点组合在 12 类 Reeds-Shepp 路径中找不到可行解。理解规划函数与输出含义示例背后只有一个对外函数xs, ys, yaws, modes, lengths m.reeds_shepp_path_planning( sx, sy, syaw, # 起点 x, y, 航向角 [rad] gx, gy, gyaw, # 终点 x, y, 航向角 [rad] maxc, # 最大曲率 [1/m] step_size0.2) # 插值步长默认 0.2参数和返回值直接来自源码含义如下sx, sy, syaw与gx, gy, gyaw起终点位姿航向角用弧度。maxc车辆最大曲率。数学文档说明推导假设曲率半径为单位长度、起点在原点代码内部会“去除起点偏移并把长度归一化后再调用各类路径函数”见 数学说明文档所以调用方只需传入真实坐标和真实曲率。返回的xs, ys, yaws按step_size插值后的路径点坐标与航向角。modes段类型序列取值S直行、L左转、R右转。lengths各段长度。负值表示该段需要倒着行驶——这就是“允许倒车”的体现。Path类中还保存了directions1: forward, -1: backward逐点方向标记。函数内部对 12 类路径函数CSC、CCC、CCCC、CCSC、CSCC、CCSCC 及其镜像/时间反演变体逐一求解候选路径去重后按总长取最小best_path_index paths.index(min(paths, keylambda p: abs(p.L)))。选不出路径时返回 5 个None。在自己的脚本中复用该函数可以从仓库根目录按测试文件同款方式导入import numpy as np from PathPlanning.ReedsSheppPath import reeds_shepp_path_planning as m m.show_animation False # 无显示环境时关闭画图 xs, ys, yaws, modes, lengths m.reeds_shepp_path_planning( -1.0, -4.0, np.deg2rad(-20.0), 5.0, 5.0, np.deg2rad(25.0), 0.1, 0.05) if xs is None: raise RuntimeError(No path) print(modes, lengths)示例中的起终点、曲率、步长都取自main()的默认值换成自己的起终点时只需替换前六个参数。show_animation控制是否弹出 matplotlib 窗口无 GUI 环境先置为False。验证规划结果仓库为这个模块提供了针对性测试 test_reeds_shepp_path_planning.py单独运行它即可pytest tests/test_reeds_shepp_path_planning.py仓库的完整测试入口是 runtests.shpytest tests -l -Werror --durations0会跑全部模块的测试上面这条命令只执行 Reeds-Shepp 这一个测试文件适合快速验证当前模块。测试内容有两部分可以当作你对自己运行结果的核对标准端点一致性check_edge_condition断言路径首点与起点、末点与终点在 x、y、yaw 上的偏差都不超过 0.01。长度一致性check_path_length断言sum(|lengths|)与按插值点坐标累加的多段折线长度之差不超过 0.01。测试还包含 10 组随机起终点固定随机种子 1234坐标范围 ±5 m航向角 ±90°曲率取1.0 / (np.random.rand() * 5.0)每组都要求规划成功并通过上述两项检查。如果你的自定义起终点跑出来的路径在端点或长度上对不上这两条判据说明参数或调用方式有问题。使用限制步长不能过大generate_path中若某段长度大于总长的 10% 却又小于step_size会打印Step size too large for Reeds-Shepp paths.并返回空路径。插值步长要显著小于最短段长示例中 0.05 对curvature 0.1就是按这个关系配的。该模块只做无障碍的起点-终点位姿连接代码和文档都没有障碍物避让逻辑需要绕行时用同一仓库的其他规划器。路径类型CSC、CCC 等的几何推导过程在 docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst 中有逐类说明涉及 90° 转弯段的 CCSC 类路径对u1有明确的取值条件如u1 2.0或u1 4.0才存在解不满足时该类候选自动跳过不影响其他类别的搜索。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考