尧图网页设计 
PythonRobotics 之 Dubins 路径规划最短转弯路径的算法原理、公式推导与源码实战【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics本篇技术指南围绕 PythonRobotics 仓库中 DubinsPath 模块及其配套文档 dubins_path_main.rst 展开系统讲解 Dubins 路径规划算法如何在简单小车模型simple car model约束下为任意两个二维位姿 (x, y, yaw) 生成满足最大曲率约束与切线航向角约束的最短路径。读完本文你将掌握 Dubins 路径的六种基本构型RSR、RSL、LSR、LSL、RLR、LRL及其各段弧长/直段长度的解析计算公式理解plan_dubins_path的完整调用方式与参数语义并能够将这一规划器直接复用于汽车类机器人的路径生成、RRT 采样路径连接等场景。Dubins 路径是什么为无法原地转向的小车设计的解析最短路径在真实世界中汽车类机器人包括差速受限的轮式车辆、固定翼飞行器等无法原地掉头其运动受到最大曲率约束即最小转弯半径约束和航向角连续约束。Dubins 路径正是一种针对这类简单小车模型的解析式路径规划算法它能够在两个二维位姿 (x, y, yaw) 之间生成满足上述两类约束的最短路径因此广泛用于路径规划问题中的起点-终点连接与采样树边生成。从 dubins_path_main.rst 的定义可以归纳出该算法的三条核心特性解析求解无需数值迭代搜索直接通过几何与三角函数闭式求解计算开销极小适合在规划循环内高频调用双约束保证路径上任意一点的曲率不超过给定最大值且起点、终点的切线方向yaw 角精确匹配输入位姿最短性在满足约束的所有可行候选构型中算法会自动选取总长度最短的一条作为最终路径。需要说明的是这一最短性是在路径由三种基本段型构成这一前提下的最优而非无限制的全局最优其理论基础可追溯至 Dubins 在 1957 年关于平均曲率约束下最小长度曲线的经典研究该文也列在原文档的 Reference 中。路径结构三种段型与六种构型组合Dubins 路径由三段组成每段要么是最大曲率圆弧要么是直线段。三段中每一段的类型都可以归为以下三类之一段型标记含义R右转弯Right turn按最大曲率向右转的圆弧L左转弯Left turn按最大曲率向左转的圆弧S直线段Straight由于三段中至少需要一段圆弧来改变航向任意一条可行 Dubins 路径必然属于以下六种组合之一RSR, RSL, LSR, LSL, RLR, LRL其中前四种RSR、RSL、LSR、LSL属于CSC型Curve-Straight-Curve圆弧-直线-圆弧后两种RLR、LRL属于CCC型Curve-Curve-Curve三段均为圆弧。这一分类在源码中体现为 dubins_path_planner.py 中的_PATH_TYPE_MAP字典将六种字符串类型分别映射到六个独立实现函数_PATH_TYPE_MAP {LSL: _LSL, RSR: _RSR, LSR: _LSR, RSL: _RSL, RLR: _RLR, LRL: _LRL, }规划器会对这六个构型逐一求解若能解出构型可行则计算其总长度最终选择总长度最短的构型作为输出路径并同时输出每段的类型与每段的距离。下图展示了 RSR 型 Dubins 路径的典型形态起点x_s与终点x_e处的箭头表示各自的航向红色实线为生成的路径黑色虚线圆为转弯轨迹示意三段距离分别标注为d1、d2、d3核心公式一CSC 型路径的段长计算——以 RSR 为例对于 RSR 这类圆弧-直线-圆弧构型三段各自的长度可以由输入位姿直接解析求出。原文档给出了完整的推导公式这里原样保留并结合源码实现展开说明。首先定义角度变量。令θ为从起点x_s指向终点x_e的向量方向角tangentd为x_s到x_e的直线距离。则α mod(-θ) β mod(x_{e,yaw} - θ)其中mod()表示角度取模运算。接着计算中间量p² 2 d² - 2cos(α-β) 2d(sin α - sin β) t atan2(cos β - cos α, d sin α - sin β)于是 RSR 路径的三段距离为d1 mod(-α t) # 第一段右转圆弧长度 d2 p # 第二段直线段长度 d3 mod(β - t) # 第三段右转圆弧长度对照源码RSR 的实现函数_RSR与文档公式完全一致def _RSR(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab _calc_trig_funcs(alpha, beta) mode [R, S, R] p_squared 2 d ** 2 - (2 * cos_ab) (2 * d * (sin_b - sin_a)) if p_squared 0: return None, None, None, mode tmp atan2((cos_a - cos_b), d - sin_a sin_b) d1 _mod2pi(alpha - tmp) d2 sqrt(p_squared) d3 _mod2pi(-beta tmp) return d1, d2, d3, mode值得注意的工程细节源码在p_squared 0时直接返回None表示该构型在当前位姿配置下不可行例如起点与终点距离过近导致无法插入直线段同时角度全部通过_mod2pi()归一化到[0, 2π)区间其底层是 utils/angle.py 中angle_mod(x, zero_2_2piTrue)将角度取模范围切换为[0, 2π)从而保证段长非负且具有明确的几何意义。核心公式二CCC 型路径的段长计算——以 RLR 为例当起点与终点位姿使得 CSC 型构型不可行时典型场景是两点距离小于 4 倍最小转弯半径、且航向差较大必须通过绕圈来完成换向需要采用三段全为圆弧的 CCC 型路径典型代表是 RLR右-左-右。原文档给出的 RLR 段长公式如下t (6.0 - d² 2cos(α-β) 2d(sin α - sin β)) / 8.0 d2 mod(2π - acos(t)) d1 mod(α - atan2(cos β - cos α, d sin α - sin β) d2 / 2.0) d3 mod(α - β - d1 d2)其中t是用于判定可行性与求解中间圆弧长度的三角量。对照源码_RLR实现 的关键判断是abs(tmp) 1.0时返回None——因为tmp将作为acos()的自变量其绝对值必须不超过 1否则说明该 CCC 构型不可行def _RLR(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab _calc_trig_funcs(alpha, beta) mode [R, L, R] tmp (6.0 - d ** 2 2.0 * cos_ab 2.0 * d * (sin_a - sin_b)) / 8.0 if abs(tmp) 1.0: return None, None, None, mode d2 _mod2pi(2 * pi - acos(tmp)) d1 _mod2pi(alpha - atan2(cos_a - cos_b, d - sin_a sin_b) d2 / 2.0) d3 _mod2pi(alpha - beta - d1 d2) return d1, d2, d3, modeRLR 型路径的形态如下三段均为圆弧d1右转、d2左转、d3右转不存在直线段由此可以看到六种构型的实现遵循统一的模式每种函数接收(alpha, beta, d)三个参数返回(d1, d2, d3, mode)或None不可行。在求得各段长度之后规划器利用最大曲率信息与各段类型即可把三段几何要素拼接还原成一条连续的离散路径点序列。最短路径的选择机制六种构型各有其适用场景CSC 型RSR、RSL、LSR、LSL适用于两点距离较远、有足够空间插入直线段的情形CCC 型RLR、LRL适用于两点距离较近、必须用圆弧绕行换向的情形。规划器不会预先假设哪一种最优而是全部尝试、取最短。这一机制在_dubins_path_planning_from_origin中实现best_cost float(inf) b_d1, b_d2, b_d3, b_mode None, None, None, None for planner in planning_funcs: d1, d2, d3, mode planner(alpha, beta, d) if d1 is None: continue cost (abs(d1) abs(d2) abs(d3)) if best_cost cost: # Select minimum length one. b_d1, b_d2, b_d3, b_mode, best_cost d1, d2, d3, mode, cost其中planning_funcs由调用方传入若不指定selected_types则使用_PATH_TYPE_MAP.values()覆盖全部六种构型若指定则只对选中的构型求解例如selected_types[RSL]时只尝试 RSL 一种。最终以三段长度绝对值之和作为代价选出代价最小的构型及其三段长度。值得一提的是源码中还做了一步坐标归一化处理plan_dubins_path开头部分利用rot_mat_2d把全局坐标系下的终点位姿旋转变换到以起点为原点、以起点航向为 x 轴的局部坐标系中在局部坐标系完成全部解析计算后再旋转平移回全局坐标。这样做的好处是让六个构型函数只需处理从原点出发这一种标准情形大幅简化公式推导与实现。源码实战plan_dubins_path 的完整调用指南函数签名与参数语义Dubins 路径规划器的对外统一入口是plan_dubins_path其完整签名如下def plan_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature, step_size0.1, selected_typesNone):各参数含义与取值范围整理如下参数类型含义说明s_x,s_yfloat起点位置 [m]二维坐标s_yawfloat起点航向角 [rad]弧度制可用np.deg2rad()转换g_x,g_yfloat终点位置 [m]二维坐标g_yawfloat终点航向角 [rad]弧度制curvaturefloat最大曲率 [1/m]其倒数即最小转弯半径取值越大转弯越急路径越短step_sizefloat可选相邻路径点间距 [m]默认0.1决定输出路径点的稠密程度selected_typeslist[str] 或 None可选限定使用的构型集合默认None表示六种全用并取最短可传如[RSL, RSR]函数返回五个值x_list路径 x 坐标序列、y_list路径 y 坐标序列、yaw_list路径各点航向角序列、modes所选构型的段型列表如[R, S, R]、lengths各段长度列表单位与输入坐标一致。可直接运行的最小示例原文档通过autofunction指令直接引用了plan_dubins_path的 docstring 作为代码指南其中包含一段可直接运行的示例。将其整理为完整脚本如下与 dubins_path_planner.py 的 main 函数 等价import numpy as np import matplotlib.pyplot as plt from PathPlanning.DubinsPath import dubins_path_planner from utils.plot import plot_arrow 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 # [1/m] path_x, path_y, path_yaw, mode, lengths dubins_path_planner.plan_dubins_path( start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) plt.plot(path_x, path_y, labelfinal course .join(mode)) plot_arrow(start_x, start_y, start_yaw) plot_arrow(end_x, end_y, end_yaw) plt.legend() plt.grid(True) plt.axis(equal) plt.show()该示例的运行结果如下图所示图中标注的构型为 LSL即左转-直线-左转起点与终点处的箭头分别表示输入航向蓝色实线为规划出的最短 Dubins 路径直接运行模块本身即可复现该图python PathPlanning/DubinsPath/dubins_path_planner.py终端会输出 Dubins path planner sample start!! 并弹出可视化窗口。常用变体用法限定构型当应用场景对转弯方向有硬性约束时例如仅允许左转可通过selected_types收窄搜索空间path_x, path_y, path_yaw, mode, lengths dubins_path_planner.plan_dubins_path( start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, selected_types[RSL])调节路径点密度增大step_size可减少路径点数量降低下游轨迹跟踪的计算量减小则路径更平滑细密默认值0.1[m] 对绝大多数小车场景足够。路径点生成的底层原理解析计算出三段长度与段型后还需把几何描述采样成离散路径点这一过程由_generate_local_course与_interpolate完成直线段S沿当前航向以length / max_curvature为步长推进航向保持不变圆弧段L/R按圆弧插值公式累加转角——左转L航向增加length右转R航向减少length位置由sin(length) / max_curvature与(1 - cos(length)) / max_curvature右转为负组合得到。采样时按step_size等间距推进并在每段末尾补齐精确端点从而保证路径严格通过起点与终点。质量保障测试用例如何验证规划结果仓库为 Dubins 路径规划器提供了专门的测试文件 tests/test_dubins_path_planning.py从三个维度验证实现正确性端点约束校验check_edge_condition断言路径首点与起点、末点与终点的位置误差均不超过 0.01 m航向角误差不超过 0.01 rad——这直接对应路径必须严格满足起点/终点位姿约束的算法要求路径长度一致性校验check_path_length对相邻路径点逐段求欧氏距离并求和断言其与各段长度之和的误差不超过 0.1 m——验证了解析段长与采样路径的一致性覆盖性测试test_3用固定随机种子np.random.seed(12345)生成 10 组随机位姿与随机曲率1/(rand*5)即最小转弯半径在 0~5 m 之间变化进行批量验证test_path_plannings_types则验证selected_types[RSL]时返回的mode精确等于[R, S, L]。这些测试通过conftest.run_this_test(__file__)组织运行也可单独执行python tests/test_dubins_path_planning.py在仓库中的实际应用Dubins 路径与其他模块的协同Dubins 路径规划器并非孤立存在它作为满足曲率约束的位姿连接器被仓库中多个 RRT 系算法复用是理解这些算法的重要前置知识RRTDubins/rrt_dubins.pyRRT 采样树在扩展新节点时使用plan_dubins_path在采样点与最近节点之间生成满足曲率约束的边而非简单的直线连接使随机树生成的路径天然可被真实小车执行RRTStarDubins/rrt_star_dubins.pyRRT* 的渐进优化版本同样借助 Dubins 路径完成节点重连与路径代价评估。换言之掌握plan_dubins_path的输入输出约定是进一步阅读这些曲率约束 RRT 变体的钥匙。此外Dubins 路径的输出路径点序列 航向序列 段长可以直接作为 PathTracking 模块中纯跟踪pure pursuit、Stanley 等轨迹跟踪控制器的参考轨迹输入从而构成规划-跟踪的完整闭环。延伸阅读原文档末尾给出了以下经典资料可作为深入理解 Dubins 路径数学原理的起点以下仅为文献指引可在公开学术渠道检索Dubins 原始论文On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents关于平均曲率约束下最小长度曲线的基础文献Wikipedia 的Dubins path词条路径定义与构型分类的综述Steven M. LaValle《Planning Algorithms》第 15.3.1 节Dubins Curves教科书级推导A Comprehensive, Step-by-Step Tutorial to Computing Dubins Paths面向实现的逐步计算教程。结合本文的公式与 dubins_path_planner.py 源码逐行对照阅读可以快速建立从数学到代码的完整映射。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 PythonRobotics 之 Dubins 路径规划：最短转弯路径的算法原理、公式推导与源码实战
PythonRobotics 之 Dubins 路径规划最短转弯路径的算法原理、公式推导与源码实战【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics本篇技术指南围绕 PythonRobotics 仓库中 DubinsPath 模块及其配套文档 dubins_path_main.rst 展开系统讲解 Dubins 路径规划算法如何在简单小车模型simple car model约束下为任意两个二维位姿 (x, y, yaw) 生成满足最大曲率约束与切线航向角约束的最短路径。读完本文你将掌握 Dubins 路径的六种基本构型RSR、RSL、LSR、LSL、RLR、LRL及其各段弧长/直段长度的解析计算公式理解plan_dubins_path的完整调用方式与参数语义并能够将这一规划器直接复用于汽车类机器人的路径生成、RRT 采样路径连接等场景。Dubins 路径是什么为无法原地转向的小车设计的解析最短路径在真实世界中汽车类机器人包括差速受限的轮式车辆、固定翼飞行器等无法原地掉头其运动受到最大曲率约束即最小转弯半径约束和航向角连续约束。Dubins 路径正是一种针对这类简单小车模型的解析式路径规划算法它能够在两个二维位姿 (x, y, yaw) 之间生成满足上述两类约束的最短路径因此广泛用于路径规划问题中的起点-终点连接与采样树边生成。从 dubins_path_main.rst 的定义可以归纳出该算法的三条核心特性解析求解无需数值迭代搜索直接通过几何与三角函数闭式求解计算开销极小适合在规划循环内高频调用双约束保证路径上任意一点的曲率不超过给定最大值且起点、终点的切线方向yaw 角精确匹配输入位姿最短性在满足约束的所有可行候选构型中算法会自动选取总长度最短的一条作为最终路径。需要说明的是这一最短性是在路径由三种基本段型构成这一前提下的最优而非无限制的全局最优其理论基础可追溯至 Dubins 在 1957 年关于平均曲率约束下最小长度曲线的经典研究该文也列在原文档的 Reference 中。路径结构三种段型与六种构型组合Dubins 路径由三段组成每段要么是最大曲率圆弧要么是直线段。三段中每一段的类型都可以归为以下三类之一段型标记含义R右转弯Right turn按最大曲率向右转的圆弧L左转弯Left turn按最大曲率向左转的圆弧S直线段Straight由于三段中至少需要一段圆弧来改变航向任意一条可行 Dubins 路径必然属于以下六种组合之一RSR, RSL, LSR, LSL, RLR, LRL其中前四种RSR、RSL、LSR、LSL属于CSC型Curve-Straight-Curve圆弧-直线-圆弧后两种RLR、LRL属于CCC型Curve-Curve-Curve三段均为圆弧。这一分类在源码中体现为 dubins_path_planner.py 中的_PATH_TYPE_MAP字典将六种字符串类型分别映射到六个独立实现函数_PATH_TYPE_MAP {LSL: _LSL, RSR: _RSR, LSR: _LSR, RSL: _RSL, RLR: _RLR, LRL: _LRL, }规划器会对这六个构型逐一求解若能解出构型可行则计算其总长度最终选择总长度最短的构型作为输出路径并同时输出每段的类型与每段的距离。下图展示了 RSR 型 Dubins 路径的典型形态起点x_s与终点x_e处的箭头表示各自的航向红色实线为生成的路径黑色虚线圆为转弯轨迹示意三段距离分别标注为d1、d2、d3核心公式一CSC 型路径的段长计算——以 RSR 为例对于 RSR 这类圆弧-直线-圆弧构型三段各自的长度可以由输入位姿直接解析求出。原文档给出了完整的推导公式这里原样保留并结合源码实现展开说明。首先定义角度变量。令θ为从起点x_s指向终点x_e的向量方向角tangentd为x_s到x_e的直线距离。则α mod(-θ) β mod(x_{e,yaw} - θ)其中mod()表示角度取模运算。接着计算中间量p² 2 d² - 2cos(α-β) 2d(sin α - sin β) t atan2(cos β - cos α, d sin α - sin β)于是 RSR 路径的三段距离为d1 mod(-α t) # 第一段右转圆弧长度 d2 p # 第二段直线段长度 d3 mod(β - t) # 第三段右转圆弧长度对照源码RSR 的实现函数_RSR与文档公式完全一致def _RSR(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab _calc_trig_funcs(alpha, beta) mode [R, S, R] p_squared 2 d ** 2 - (2 * cos_ab) (2 * d * (sin_b - sin_a)) if p_squared 0: return None, None, None, mode tmp atan2((cos_a - cos_b), d - sin_a sin_b) d1 _mod2pi(alpha - tmp) d2 sqrt(p_squared) d3 _mod2pi(-beta tmp) return d1, d2, d3, mode值得注意的工程细节源码在p_squared 0时直接返回None表示该构型在当前位姿配置下不可行例如起点与终点距离过近导致无法插入直线段同时角度全部通过_mod2pi()归一化到[0, 2π)区间其底层是 utils/angle.py 中angle_mod(x, zero_2_2piTrue)将角度取模范围切换为[0, 2π)从而保证段长非负且具有明确的几何意义。核心公式二CCC 型路径的段长计算——以 RLR 为例当起点与终点位姿使得 CSC 型构型不可行时典型场景是两点距离小于 4 倍最小转弯半径、且航向差较大必须通过绕圈来完成换向需要采用三段全为圆弧的 CCC 型路径典型代表是 RLR右-左-右。原文档给出的 RLR 段长公式如下t (6.0 - d² 2cos(α-β) 2d(sin α - sin β)) / 8.0 d2 mod(2π - acos(t)) d1 mod(α - atan2(cos β - cos α, d sin α - sin β) d2 / 2.0) d3 mod(α - β - d1 d2)其中t是用于判定可行性与求解中间圆弧长度的三角量。对照源码_RLR实现 的关键判断是abs(tmp) 1.0时返回None——因为tmp将作为acos()的自变量其绝对值必须不超过 1否则说明该 CCC 构型不可行def _RLR(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab _calc_trig_funcs(alpha, beta) mode [R, L, R] tmp (6.0 - d ** 2 2.0 * cos_ab 2.0 * d * (sin_a - sin_b)) / 8.0 if abs(tmp) 1.0: return None, None, None, mode d2 _mod2pi(2 * pi - acos(tmp)) d1 _mod2pi(alpha - atan2(cos_a - cos_b, d - sin_a sin_b) d2 / 2.0) d3 _mod2pi(alpha - beta - d1 d2) return d1, d2, d3, modeRLR 型路径的形态如下三段均为圆弧d1右转、d2左转、d3右转不存在直线段由此可以看到六种构型的实现遵循统一的模式每种函数接收(alpha, beta, d)三个参数返回(d1, d2, d3, mode)或None不可行。在求得各段长度之后规划器利用最大曲率信息与各段类型即可把三段几何要素拼接还原成一条连续的离散路径点序列。最短路径的选择机制六种构型各有其适用场景CSC 型RSR、RSL、LSR、LSL适用于两点距离较远、有足够空间插入直线段的情形CCC 型RLR、LRL适用于两点距离较近、必须用圆弧绕行换向的情形。规划器不会预先假设哪一种最优而是全部尝试、取最短。这一机制在_dubins_path_planning_from_origin中实现best_cost float(inf) b_d1, b_d2, b_d3, b_mode None, None, None, None for planner in planning_funcs: d1, d2, d3, mode planner(alpha, beta, d) if d1 is None: continue cost (abs(d1) abs(d2) abs(d3)) if best_cost cost: # Select minimum length one. b_d1, b_d2, b_d3, b_mode, best_cost d1, d2, d3, mode, cost其中planning_funcs由调用方传入若不指定selected_types则使用_PATH_TYPE_MAP.values()覆盖全部六种构型若指定则只对选中的构型求解例如selected_types[RSL]时只尝试 RSL 一种。最终以三段长度绝对值之和作为代价选出代价最小的构型及其三段长度。值得一提的是源码中还做了一步坐标归一化处理plan_dubins_path开头部分利用rot_mat_2d把全局坐标系下的终点位姿旋转变换到以起点为原点、以起点航向为 x 轴的局部坐标系中在局部坐标系完成全部解析计算后再旋转平移回全局坐标。这样做的好处是让六个构型函数只需处理从原点出发这一种标准情形大幅简化公式推导与实现。源码实战plan_dubins_path 的完整调用指南函数签名与参数语义Dubins 路径规划器的对外统一入口是plan_dubins_path其完整签名如下def plan_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature, step_size0.1, selected_typesNone):各参数含义与取值范围整理如下参数类型含义说明s_x,s_yfloat起点位置 [m]二维坐标s_yawfloat起点航向角 [rad]弧度制可用np.deg2rad()转换g_x,g_yfloat终点位置 [m]二维坐标g_yawfloat终点航向角 [rad]弧度制curvaturefloat最大曲率 [1/m]其倒数即最小转弯半径取值越大转弯越急路径越短step_sizefloat可选相邻路径点间距 [m]默认0.1决定输出路径点的稠密程度selected_typeslist[str] 或 None可选限定使用的构型集合默认None表示六种全用并取最短可传如[RSL, RSR]函数返回五个值x_list路径 x 坐标序列、y_list路径 y 坐标序列、yaw_list路径各点航向角序列、modes所选构型的段型列表如[R, S, R]、lengths各段长度列表单位与输入坐标一致。可直接运行的最小示例原文档通过autofunction指令直接引用了plan_dubins_path的 docstring 作为代码指南其中包含一段可直接运行的示例。将其整理为完整脚本如下与 dubins_path_planner.py 的 main 函数 等价import numpy as np import matplotlib.pyplot as plt from PathPlanning.DubinsPath import dubins_path_planner from utils.plot import plot_arrow 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 # [1/m] path_x, path_y, path_yaw, mode, lengths dubins_path_planner.plan_dubins_path( start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) plt.plot(path_x, path_y, labelfinal course .join(mode)) plot_arrow(start_x, start_y, start_yaw) plot_arrow(end_x, end_y, end_yaw) plt.legend() plt.grid(True) plt.axis(equal) plt.show()该示例的运行结果如下图所示图中标注的构型为 LSL即左转-直线-左转起点与终点处的箭头分别表示输入航向蓝色实线为规划出的最短 Dubins 路径直接运行模块本身即可复现该图python PathPlanning/DubinsPath/dubins_path_planner.py终端会输出 Dubins path planner sample start!! 并弹出可视化窗口。常用变体用法限定构型当应用场景对转弯方向有硬性约束时例如仅允许左转可通过selected_types收窄搜索空间path_x, path_y, path_yaw, mode, lengths dubins_path_planner.plan_dubins_path( start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, selected_types[RSL])调节路径点密度增大step_size可减少路径点数量降低下游轨迹跟踪的计算量减小则路径更平滑细密默认值0.1[m] 对绝大多数小车场景足够。路径点生成的底层原理解析计算出三段长度与段型后还需把几何描述采样成离散路径点这一过程由_generate_local_course与_interpolate完成直线段S沿当前航向以length / max_curvature为步长推进航向保持不变圆弧段L/R按圆弧插值公式累加转角——左转L航向增加length右转R航向减少length位置由sin(length) / max_curvature与(1 - cos(length)) / max_curvature右转为负组合得到。采样时按step_size等间距推进并在每段末尾补齐精确端点从而保证路径严格通过起点与终点。质量保障测试用例如何验证规划结果仓库为 Dubins 路径规划器提供了专门的测试文件 tests/test_dubins_path_planning.py从三个维度验证实现正确性端点约束校验check_edge_condition断言路径首点与起点、末点与终点的位置误差均不超过 0.01 m航向角误差不超过 0.01 rad——这直接对应路径必须严格满足起点/终点位姿约束的算法要求路径长度一致性校验check_path_length对相邻路径点逐段求欧氏距离并求和断言其与各段长度之和的误差不超过 0.1 m——验证了解析段长与采样路径的一致性覆盖性测试test_3用固定随机种子np.random.seed(12345)生成 10 组随机位姿与随机曲率1/(rand*5)即最小转弯半径在 0~5 m 之间变化进行批量验证test_path_plannings_types则验证selected_types[RSL]时返回的mode精确等于[R, S, L]。这些测试通过conftest.run_this_test(__file__)组织运行也可单独执行python tests/test_dubins_path_planning.py在仓库中的实际应用Dubins 路径与其他模块的协同Dubins 路径规划器并非孤立存在它作为满足曲率约束的位姿连接器被仓库中多个 RRT 系算法复用是理解这些算法的重要前置知识RRTDubins/rrt_dubins.pyRRT 采样树在扩展新节点时使用plan_dubins_path在采样点与最近节点之间生成满足曲率约束的边而非简单的直线连接使随机树生成的路径天然可被真实小车执行RRTStarDubins/rrt_star_dubins.pyRRT* 的渐进优化版本同样借助 Dubins 路径完成节点重连与路径代价评估。换言之掌握plan_dubins_path的输入输出约定是进一步阅读这些曲率约束 RRT 变体的钥匙。此外Dubins 路径的输出路径点序列 航向序列 段长可以直接作为 PathTracking 模块中纯跟踪pure pursuit、Stanley 等轨迹跟踪控制器的参考轨迹输入从而构成规划-跟踪的完整闭环。延伸阅读原文档末尾给出了以下经典资料可作为深入理解 Dubins 路径数学原理的起点以下仅为文献指引可在公开学术渠道检索Dubins 原始论文On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents关于平均曲率约束下最小长度曲线的基础文献Wikipedia 的Dubins path词条路径定义与构型分类的综述Steven M. LaValle《Planning Algorithms》第 15.3.1 节Dubins Curves教科书级推导A Comprehensive, Step-by-Step Tutorial to Computing Dubins Paths面向实现的逐步计算教程。结合本文的公式与 dubins_path_planner.py 源码逐行对照阅读可以快速建立从数学到代码的完整映射。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考