PythonRobotics 如何用 Dubins 路径规划生成带航向约束的最短路径【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics如果你的移动平台小车、无人机、AGV 之类不能原地转向从一个位姿到另一个位姿时既要求起点航向、也要求终点航向同时受最大转弯能力限制就需要 Dubins 路径规划。PythonRobotics 仓库中的 Dubins 路径规划器 就是这个场景的实现它接受两个二维位姿 (x, y, yaw) 和最大曲率从 6 种解析路径类型中选出总长度最短的一条输出整条路径的坐标序列。这篇文章基于仓库自带的文档与示例代码讲清楚如何跑起来、每个参数代表什么、以及怎么用返回值验证结果。运行环境准备按照仓库的 运行示例代码指南示例代码推荐在Python 3.12.x下运行旧版本 Python 3 可能也能跑但没有经过测试。依赖通过仓库requirements目录下的清单文件安装二选一# 使用 pip pip install -r requirements/requirements.txt # 或使用 conda需要已安装 Anaconda/Miniconda conda env create -f requirements/environment.ymlrequirements/requirements.txt 中列出的核心依赖为numpy、scipy、matplotlib另有cvxpy及测试用的pytest、mypy、ruff。运行 Dubins 示例实际用到的是numpy和matplotlib。Dubins 路径规划器在做什么算法说明文档给出的定义是Dubins 路径是面向简单车辆模型的解析规划算法在最大曲率约束和切线yaw 角约束下生成两个二维位姿 (x, y, yaw) 之间的最短路径。生成的路径由三段组成每段只能是最大曲率圆弧或直线段段类型标记为R右转弯L左转弯S直线因此可能的路径就是 6 种组合之一RSR、RSL、LSR、LSL、RLR、LRL。文档中给出了 RSR 和 RLR 两类路径的几何关系与分段长度推导基于起始/终点切线角 α、β 与起点到终点的距离 d。规划逻辑在源码_dubins_path_planning_from_origin中把 6 种类型全部求一遍每种类型算出三段长度d1 d2 d3cost取 cost 最小的一条作为最终路径再按step_size插值出路径点。如果某种类型无解例如中间量p_squared 0或abs(tmp) 1.0该类型直接返回None被跳过。核心接口plan_dubins_path 的参数入口函数是 plan_dubins_pathpath_x, path_y, path_yaw, mode, lengths plan_dubins_path( s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature, step_size0.1, selected_typesNone)参数含义来自函数的 docstring单位与文档一致参数单位含义s_x, s_y, s_yawm, m, rad起点位姿yaw 是航向角g_x, g_y, g_yawm, m, rad终点位姿curvature1/m最大曲率决定最小转弯半径step_sizem相邻路径点之间的步长默认 0.1selected_types字符串列表或 None限定使用哪些路径类型如[RSL, RSR]为None时 6 种全用并返回最短的一条返回值x_list/y_list路径各点的 x、y 坐标yaw_list路径各点的 yaw 角modes所选路径的段类型列表例如[R, S, R]lengths各段长度单位为米源码中先以弧度计返回前按length / curvature换算注意yaw全程使用弧度。如果你的航向是角度制先用np.deg2rad()转换示例代码就是这么写的。运行仓库自带示例示例脚本是 dubins_path_planner.pymain()中固定的输入可直接照抄作为起点start_x 1.0 # [m] start_y 1.0 # [m] start_yaw np.deg2rad(45.0) # [rad] end_x -3.0 # [m] end_y -3.0 # [m] end_yaw np.deg2rad(-45.0) # [rad] curvature 1.0 path_x, path_y, path_yaw, mode, lengths plan_dubins_path(start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)在仓库根目录下执行python PathPlanning/DubinsPath/dubins_path_planner.py终端会打印Dubins path planner sample start!!随后弹出 matplotlib 窗口画出路径曲线、起点和终点的箭头图例即为选中的路径类型由.join(mode)拼出例如RSR。这个可视化受文件顶部的show_animation True开关控制设为False时只计算不绘图。脚本开头有sys.path.append(...)把仓库根目录加入搜索路径第 8–10 行所以不需要手动设置环境变量即可直接运行。在自己的代码里调用不运行整个示例、只想拿路径数据时直接在仓库根目录下起一个 Python 进程导入即可脚本的 sys.path 处理只对它自己生效导入方需要保证仓库根目录在sys.path中例如在根目录运行import numpy as np from PathPlanning.DubinsPath import dubins_path_planner path_x, path_y, path_yaw, mode, lengths dubins_path_planner.plan_dubins_path( 1.0, 1.0, np.deg2rad(45.0), -3.0, -3.0, np.deg2rad(-45.0), 1.0) print(.join(mode), lengths)两个常用分支只用默认 6 类型选最短selected_types传None不传即可。限定路径类型可选分支例如selected_types[RSL]时只计算 RSL 一种组合。仓库测试 test_path_plannings_types 验证了这一点指定[RSL]后返回的mode [R, S, L]。如何验证结果正确仓库自带的单测 tests/test_dubins_path_planning.py 就是文档给出的核对方式两条断言边界条件路径第一个点等于起点位姿、最后一个点等于终点位姿误差 ≤ 0.01assert abs(px[0] - start_x) 0.01 assert abs(px[-1] - end_x) 0.01 # y 与 yaw 同理路径总长把路径点逐段求欧氏距离再求和与lengths各段之和相差不超过 0.1path_len sum([np.hypot(dx, dy) for (dx, dy) in zip(np.diff(px), np.diff(py))]) assert abs(path_len - sum(lengths)) 0.1你可以把这两段判断套在自己调用的返回值上做同样校验。想批量跑仓库的完整测试根目录有 runtests.sh它执行pytest tests -l -Werror --durations0跑全部模块的测试不止 Dubins 一项只验证 Dubins 时直接运行上面的测试文件即可。适用边界根据算法文档与源码这个规划器是纯几何的它没有障碍物输入参数生成的是曲率与航向约束下的最短路径不做碰撞检测需要避障的组合方案请参考仓库 PathPlanning 目录下的其他算法。另外step_size只影响输出路径点的疏密不影响路径本身的最短性curvature则直接决定转弯半径取值需结合平台的最大转弯能力设定。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
