PythonRobotics 如何用 Dubins 路径规划生成带航向约束的最短路径

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.yml

requirements/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 + d3（cost），取 cost 最小的一条作为最终路径，再按step_size插值出路径点。如果某种类型无解（例如中间量p_squared < 0或abs(tmp) > 1.0），该类型直接返回None被跳过。

核心接口：plan_dubins_path 的参数

入口函数是 plan_dubins_path：

path_x, path_y, path_yaw, mode, lengths = plan_dubins_path( s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature, step_size=0.1, selected_types=None)

参数含义（来自函数的 docstring，单位与文档一致）：

返回值：

• x_list/y_list：路径各点的 x、y 坐标
• yaw_list：路径各点的 yaw 角
• modes：所选路径的段类型列表，例如["R", "S", "R"]
• lengths：各段长度，单位为米（源码中先以弧度计，返回前按length / curvature换算）

注意yaw全程使用弧度。如果你的航向是角度制，先用np.deg2rad()转换，示例代码就是这么写的。

运行仓库自带示例

示例脚本是 dubins_path_planner.py，main()中固定的输入（可直接照抄作为起点）：

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 就是文档给出的核对方式，两条断言：

1. 边界条件：路径第一个点等于起点位姿、最后一个点等于终点位姿，误差 ≤ 0.01：

   assert abs(px[0] - start_x) <= 0.01 assert abs(px[-1] - end_x) <= 0.01 # y 与 yaw 同理

2. 路径总长：把路径点逐段求欧氏距离再求和，与lengths各段之和相差不超过 0.1：

   path_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 --durations=0跑全部模块的测试，不止 Dubins 一项；只验证 Dubins 时直接运行上面的测试文件即可。

适用边界

根据算法文档与源码，这个规划器是纯几何的：它没有障碍物输入参数，生成的是曲率与航向约束下的最短路径，不做碰撞检测；需要避障的组合方案请参考仓库 PathPlanning 目录下的其他算法。另外step_size只影响输出路径点的疏密，不影响路径本身的最短性；curvature则直接决定转弯半径，取值需结合平台的最大转弯能力设定。

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics