★ FEATURED ARTICLE 简介Dubins曲线MATLAB实现是一份面向路径规划学习者和机器人研究者的源码包解决车辆从起点到终点在转向约束下生成最短平滑路径的核心问题。包内共6个文件包括两个m主程序dubins_core.m负责核心计算dubins_curve.m负责曲线生成与绘制、README说明文档以及License等辅助文件压缩包仅9KB结构精简便于直接阅读和二次开发。已有679人学习/下载。该资源覆盖Dubins曲线的LS、RS、SP三类典型模式演示了参数化曲线、数值优化与插值绘制等关键技术通过m文件注释和示例读者可以快速理解状态空间表示与最短路径求解流程掌握在MATLAB中实现路径规划的基本方法并可直接修改起点、终点和heading角度等参数验证不同约束下的曲线形态。适合具备基础MATLAB知识、刚接触路径规划算法的学生和开发者用作入门参考。1. Dubins 曲线为什么折线路径在小车和固定翼上跑不动最早接触 Dubins curve 是在一个移动小车项目里导航模块用 A* 规划出一条折线路径每个拐点都是角度突变底盘控制器根本追不上车在两个航点之间来回画龙。后来换成 Dubins 曲线问题一下简单了很多——它输出的是「在最小转弯半径约束下两个状态之间的最短可行路径」每条路径由固定曲率的圆弧和直线段组成天生满足车辆的转向限制。这个名字在 MATLAB 社区里出现频率不低但真正手写过的人不多网上能直接跑的代码要么依赖机器人工具箱要么只画了样子没给计算过程。这篇文章把 Dubins curve 在 MATLAB 里的实现从头拆一遍六种路径组合怎么来的、圆弧和切线的坐标怎么算、参数怎么设、哪些边界条件最容易翻车。适合刚开始接触路径规划的读者也适合已经在用 A* / RRT 但觉得输出路径太「折」、想让结果更可跟踪的人。2. 六种最优组合把 Dubins 问题拆成几何可算的步骤2.1 状态、转向方向与三类基本段Dubins 曲线处理的对象是平面上的运动状态通常写作三个量位置坐标 (x, y) 和航向角 θ。输入是起点状态和终点状态外加一个约束参数 ρ代表车辆或飞行器的最小转弯半径。输出是一条由圆弧和直线拼接而成的路径使得路径上每一点的曲率绝对值都不超过 1/ρ。为什么要用「圆弧 直线」而不是任意曲线这是从运动学约束反推出来的。对于常用的自行车模型和固定翼模型瞬时转弯半径和转向角速度直接相关曲率不能突变但可以在拐弯段保持恒定。Dubins 曲线恰好在「满足曲率约束」和「路径最短」之间取了最优它在理论上证明了最短路径一定是若干段固定半径圆弧半径取最小值 ρ和直线段的组合不会出现曲率变化的渐变弧。路径中的基本段只有三类用方向来命名L左转圆弧逆时针方向曲率为 1/ρR右转圆弧顺时针方向曲率为 -1/ρS直线段曲率为 0。这三类段本身不复杂复杂的是它们的排列方式。你可以把 Dubins 路径理解成一个由 L、R、S 组成的字符串比如 RSL 表示「先右转、再直行、再左转」。约束条件有两个第一相邻两段的连接点处航向必须连续第二整条路径必须从起点状态精确到达终点状态。2.2 为什么最优解只有 LSL、LSR、RSL、RSR、LRL、RLR 六种理论上包含圆弧和直线的排列组合有很多但 Dubins 曲线的最优性证明给出了一个关键结论任意两点之间的最短路径只会出现在以下六种模式中。模式结构适用场景特点LSL左转 - 直线 - 左转起点终点朝向相近绕行方向一致RSR右转 - 直线 - 右转同 LSL方向相反LSR左转 - 直线 - 右转两段圆弧在直线两侧类似 S 形绕行RSL右转 - 直线 - 左转同 LSR方向相反LRL左转 - 右转 - 左转起点终点距离较近且朝向差异大RLR右转 - 左转 - 右转同 LRL方向相反这六种模式分为两大类。前四种属于 CSC 结构圆 - 直线 - 圆直线段位于两个圆弧之间两端的圆弧方向可以相同也可以相反。后两种属于 CCC 结构圆 - 圆 - 圆三段圆弧连续转向中间的圆弧方向与前后的相反用于处理起点终点距离很近、无法用直线衔接的情况。为什么中间没有「直线 圆弧 直线」这类组合因为如果起步就直行、最后也直行中间的圆弧只改变朝向不改变位置整体路径不会比直接用两个圆弧加一条直线更短。实际工程中没必要从零推导证明记住这两类结构和六个模式即可写 MATLAB 代码时把六种可能都算一遍取最短比手动判断哪种模式最优要稳妥得多。2.3 六种路径长度的比较逻辑实现 Dubins 曲线时有一种常见的误区是试图通过判断起点终点的相对位置直接确定模式。这样做既不通用也容易漏掉边界情况。我一般采取的办法是把六种模式对应的几何参数全部算出来得到每条候选路径的几何长度然后取长度最小的那条作为结果。这里说的「长度」不是把离散点加起来而是用解析式算出来的精确值。一条 CSC 路径的长度等于「起始圆弧弧长 直线段长度 终止圆弧弧长」一条 CCC 路径的长度等于三段圆弧弧长之和。弧长等于半径乘以转角绝对值即 ρ × |Δθ|。这个计算在 MATLAB 里只是几个标量运算六种模式枚举一遍开销极小完全不必用数值积分去逼近。枚举取最短还有一个好处当起点终点距离很近或朝向差异很特殊时某种模式可能会退化比如圆弧转角接近零解析长度公式仍然能给出可靠的比较结果。后面第 4 章会详细讲这些退化情况怎么处理这里先建立「枚举 取短」的思维框架。3. 在 MATLAB 里从零实现可运行的 Dubins 路径生成代码3.1 主流程设计枚举六种模式取最短路径代码的结构不需要很复杂。一个对外的主函数接收起点状态、终点状态、转弯半径和采样步长然后在内部枚举六个候选模式。每个模式由一个构造器函数负责生成离散点最后比较长度并返回最优路径。function [x, y, th, best_mode, total_len] dubins_path(start, goal, rho, ds) % DUBINS_PATH 生成两点之间的 Dubins 最短路径 % 输入: % start [x0, y0, th0] 起点状态 % goal [xg, yg, thg] 终点状态 % rho 最小转弯半径 % ds 路径采样步长 % 输出: % x, y, th 路径离散点坐标与航向 % best_mode 命中的最优模式字符串 % total_len 路径总长度 modes {LSL, LSR, RSL, RSR, LRL, RLR}; best_len Inf; best_data []; for k 1:6 mode modes{k}; if contains(mode, S) [px, py, pt, len] build_csc(start, goal, rho, ds, mode); else [px, py, pt, len] build_ccc(start, goal, rho, ds, mode); end if len best_len best_len len; best_data {px, py, pt, mode}; end end x best_data{1}; y best_data{2}; th best_data{3}; best_mode best_data{4}; total_len best_len; end主函数的核心逻辑是「不管黑猫白猫算完六种比长度」。CSC 和 CCC 两类路径的几何构造方式不同拆成两个构造器更清晰。这里有个容易忽略的点比较长度时用的是构造器返回的解析长度而不是离散点连线的折线长度。因为采样步长不同离散点数量也不同折线长度会引入与真实路径无关的误差而解析长度是一套独立的几何计算不受采样影响。3.2 圆弧段与直线段的生成函数构造任何模式之前先准备好两个基础函数。圆弧段的输入是圆心、半径、起始角度、要转过的角度、转向方向和采样步长直线段的输入是起点和终点坐标。function [x, y, th] arc_segment(cx, cy, r, theta_start, delta_theta, dir, ds) % 生成一段圆弧 % dir 1 逆时针(左转) % dir -1 顺时针(右转) % delta_theta 为弧段对应圆心角(正值) n max(2, ceil(abs(delta_theta) * r / ds)); ang theta_start dir * linspace(0, delta_theta, n 1); x cx r * cos(ang); y cy r * sin(ang); % 车头朝向 径向方向 转向方向的 90 度 head ang dir * pi / 2; th wrap_angle(head); end function [x, y, th] line_segment(p1, p2, ds) % 生成一段直线 dx p2(1) - p1(1); dy p2(2) - p1(2); L sqrt(dx^2 dy^2); n max(2, ceil(L / ds)); t linspace(0, 1, n 1); x p1(1) dx * t; y p1(2) dy * t; th atan2(dy, dx) * ones(1, n 1); end function th wrap_angle(th) % 把角度折算到 [-pi, pi) th mod(th pi, 2 * pi) - pi; end圆弧段的参数里delta_theta 取正值转向由 dir 决定。这样设计的好处是在外部只需要关心「从哪个角度开始、一共转多大角度」不需要传入负角度避免角度符号和转向方向两重负号叠加导致混乱。车头朝向的计算依据是逆时针圆弧上某点的切线方向等于径向角加 90 度顺时针则是减 90 度。采样点数 n 的计算公式里加了 max(2, ...) 的保护防止圆弧极短甚至接近零时出现只有一个点的情况。直线段同理即使两点重合也要至少输出两个点这样后续拼接离散点不会产生空数组。wrap_angle 自己实现避免依赖 MATLAB 的 Mapping Toolbox 里的 wrapToPi。3.3 CSC 模式外切线与内切线的切点计算CSC 结构是「圆弧 - 直线 - 圆弧」难点在找两个圆之间的公切线。左右转圆弧构成的两个圆半径都为 ρ圆心位置由起点终点状态直接算出左转圆在起点左侧 ρ 处右转圆在起点右侧 ρ 处终点同理。function [cx, cy] circle_center(x, y, th, dir, rho) % 根据当前状态和转向方向计算圆弧圆心 if dir 1 % 左转 cx x rho * cos(th pi/2); cy y rho * sin(th pi/2); else % 右转 cx x rho * cos(th - pi/2); cy y rho * sin(th - pi/2); end end得到起点圆弧圆心 c1 和终点圆弧圆心 c3 之后模式的首尾字母决定了使用外切线还是内切线。LSL 和 RSR 两端圆弧转向方向相同连接两个圆的外侧切线LSR 和 RSL 两端转向方向相反需要连接内侧切线。function [t1, t2] outer_tangent(c1, c3, rho, mode) % 半径相等两圆的外切线切点 d_vec c3 - c1; alpha atan2(d_vec(2), d_vec(1)); if strcmp(mode, LSL) % 下方外切线 t1 c1 rho * [sin(alpha), -cos(alpha)]; t2 c3 rho * [sin(alpha), -cos(alpha)]; else % RSR 上方外切线 t1 c1 rho * [-sin(alpha), cos(alpha)]; t2 c3 rho * [-sin(alpha), cos(alpha)]; end end function [t1, t2] inner_tangent(c1, c3, rho, side) % 半径相等两圆的内切线切点, side1/-1 选择两条之一 d_vec c3 - c1; d norm(d_vec); if d 2 * rho error(两圆心距离过近内切线不存在); end alpha atan2(d_vec(2), d_vec(1)); beta asin(2 * rho / d); theta_c alpha - side * beta; % 公切线方向 theta_r theta_c pi / 2; % 切点径向方向 t1 c1 rho * [cos(theta_r), sin(theta_r)]; t2 c2 rho * [cos(theta_r pi), sin(theta_r pi)]; end外切线的公式看着简单但方向容易搞混。以 LSL 为例当时我推导时反复验证过车在切点处的航向等于 alpha两圆心连线方向左转圆上航向等于径向角加 90 度所以径向角必须是 alpha 减 90 度对应切点相对圆心在右下方。如果写成相反的方向路径会绕远一个大圈。内切线公式里的 side 参数LSL 模式用 1 还是 -1取决于你期望车辆从哪个方向切入实测时两种都试一下取长度短的那个即可——主函数本来就枚举全部模式这一层天然有保护。完整的 build_csc 构造器负责把三个部分拼起来起点圆弧从起始角转到切点 1直线从切点 1 到切点 2终点圆弧从切点 2 转到终点角。function [x, y, th, len] build_csc(start, goal, rho, ds, mode) x0 start(1); y0 start(2); th0 start(3); xg goal(1); yg goal(2); thg goal(3); % 根据模式首尾字母确定圆心 if mode(1) L [c1x, c1y] circle_center(x0, y0, th0, 1, rho); else [c1x, c1y] circle_center(x0, y0, th0, -1, rho); end if mode(3) L [c3x, c3y] circle_center(xg, yg, thg, 1, rho); else [c3x, c3y] circle_center(xg, yg, thg, -1, rho); end c1 [c1x, c1y]; c3 [c3x, c3y]; if strcmp(mode, LSL) || strcmp(mode, RSR) [t1, t2] outer_tangent(c1, c3, rho, mode); else % LSR 与 RSL 的内切线side 由模式决定 side 1; if strcmp(mode, RSL), side -1; end [t1, t2] inner_tangent(c1, c3, rho, side); end % 计算各段转角 th_c1 atan2(t1(2)-c1y, t1(1)-c1x); % 起点圆弧终点径向角 th_c3 atan2(t2(2)-c3y, t2(1)-c3x); % 终点圆弧起点径向角 delta1 wrap_angle(th_c1 - th0); % 起点圆弧转角 if mode(1) L delta1 wrap_angle(th_c1 - (th0 pi/2)); else delta1 wrap_angle(th_c1 - (th0 - pi/2)); end % 为保证正角度, 与转向方向对齐后取绝对值 % ... end写到这里细心的读者会发现转角计算最绕。我的建议是在 build_csc 内部先计算「从起点圆心角转到切点圆心角」的差值再根据转向方向取正确的符号。具体做的时候把圆弧段的起止角度都统一成圆心角来算避免混合使用航向角和圆心角。后面第 5 章的避坑部分会专门讲这个位置最容易出的问题。3.4 CCC 模式中间圆的几何关系CCC 结构处理的是起点终点距离较近、直线段无法衔接的情况。三段圆弧的半径都取 ρ中间圆弧方向与两端相反。几何关键在中间圆的圆心位置它到 c1 和 c3 的距离都等于 2ρ。function [x, y, th, len] build_ccc(start, goal, rho, ds, mode) x0 start(1); y0 start(2); th0 start(3); xg goal(1); yg goal(2); thg goal(3); if mode(1) L [c1x, c1y] circle_center(x0, y0, th0, 1, rho); else [c1x, c1y] circle_center(x0, y0, th0, -1, rho); end if mode(3) L [c3x, c3y] circle_center(xg, yg, thg, 1, rho); else [c3x, c3y] circle_center(xg, yg, thg, -1, rho); end d_vec [c3x - c1x, c3y - c1y]; d norm(d_vec); if d 4 * rho error(两圆心距离过远CCC 模式不存在); end % 中间圆心在垂直平分线上, 距中点 h mid (c1 c3) / 2; e d_vec / d; h sqrt(max(0, 4*rho^2 - (d/2)^2)); n_vec [-e(2), e(1)]; if strcmp(mode, LRL) c2 mid h * n_vec; else c2 mid - h * n_vec; end % 三段圆弧各自的角度按几何关系计算 % 再调用 arc_segment 拼接 % ... end中间圆圆心公式里有一个细节h 的计算用了 max(0, ...)这是为了处理 d 恰好等于 4ρ 的边界。理论上 h 为 0 时公式依然有效但浮点运算可能让根号里出现一个极小的负值导致结果为 NaN。加一层保护是值得的。CCC 模式中中间圆弧的转角计算比 CSC 更绕因为它涉及两次相切点。实际编码时可以用一个技巧先不急着算角度而是把三段圆弧的切点坐标求出来再用 atan2 反推每个切点在各自圆上的圆心角这样逻辑最直白。3.5 完整调用示例与可视化把前面的函数组合起来一个完整的调用和绘图流程如下% 测试 Dubins 路径生成 start [0, 0, pi/6]; % 起点位置 (0,0), 航向 30 度 goal [30, 20, -pi/4]; % 终点位置 (30,20), 航向 -45 度 rho 5; % 最小转弯半径 5 米 ds 0.2; % 采样步长 0.2 米 [x, y, th, mode, len] dubins_path(start, goal, rho, ds); figure; plot(x, y, b-, LineWidth, 2); hold on; % 绘制起点终点方向 quiver(start(1), start(2), cos(start(3)), sin(start(3)), 2, r); quiver(goal(1), goal(2), cos(goal(3)), sin(goal(3)), 2, g); axis equal; grid on; title([最优模式: , mode, 路径长度: , num2str(len)]);代码里的 quiver 用来画起点终点的航向箭头axis equal 保证圆弧不会因为坐标轴比例不均而看起来变形。这一步很多人会漏坐标轴不等比时正确的圆弧在图上会被拉成椭圆肉眼判断很容易误判路径质量。调用主函数之前建议先确认输入的 rho 是正值且远大于数值精度下限。如果 rho 设成 0圆弧生成函数里会出现除零。更隐蔽的情况是 rho 比 ds 还小此时圆弧段采样点数被 max(2,...) 保护几何上仍然成立但画出来的路径会严重失真——这个度放到下一章细讲。4. 参数调优与退化边界转弯半径、采样步长怎么设4.1 转弯半径先分清物理下限和数值下限Dubins 路径里的 ρ 不是随便取的它代表车辆或飞行器实际能执行的最小转弯半径。对于前轮转向车辆最小转弯半径通常从车辆手册里查或者直接实测把方向盘打死绕一圈量直径。对于差速底盘最小转弯半径理论上可以为零原地转但实际运动学里原地转会导致轮胎磨损和定位漂移工程上一般设一个大于零的下限。平台类型典型最小转弯半径备注差速小车0.3 ~ 0.8 m原地转向但路径质量差前轮转向小车1.5 ~ 4 m与轴距和最大转向角强相关固定翼无人机20 ~ 60 m取决于飞行速度和滚转极限自动泊车场景4 ~ 6 m乘用车典型值数值下限这一层很容易被忽视。如果 ρ 设成 1e-6数学上可行但圆弧曲率极大采样点数会爆炸且路径对起点终点的微小扰动极其敏感控制器的跟踪误差也会被放大。我一般会把 ρ 设成两个值中较大的那个物理最小转弯半径和「路径离散化后仍能保持平滑」的经验值比如 ρ 10 × ds。转弯半径还影响路径的可达性。LSR 和 RSL 模式要求两圆心距离 d ≥ 2ρCCC 模式要求 d ≤ 4ρ。如果 ρ 设得过大内切线可能不存在设得过小CCC 模式又可能失效。因此实际工程里 ρ 不是固定不变的可以做成可调参数在规划失败时自动放宽或收紧。4.2 采样步长路径平滑度与计算量的权衡ds 决定了离散路径点的密度。设得太大相邻点之间航向角跳变明显控制器在每个点之间都得重新规划设得太小点数量暴增后续碰撞检测和路径平滑的计算开销线性上升。经验公式是让 ds 不超过 ρ 的十分之一。比如 ρ 5 m 时ds 取 0.2 ~ 0.5 m 比较合适。如果路径是给差速小车用的还可以把 ds 绑定到底盘控制频率和最大线速度ds v_max / f_ctrl即每个控制周期车能走过的距离。一个常见反例是路径生成 ds 1 m但控制器频率 50 Hz、车速 2 m/s相邻路径点之间的行驶时间只有 0.5 秒中间还夹着 25 个控制周期控制器只能靠插值脑补跟踪效果自然不会好。采样步长对路径长度的影响很小因为主函数比较的是解析长度。但对碰撞检测的影响很大ds 过大可能跳过窄障碍物的间隙ds 过小检测耗时就上去了。需要碰撞检测时我一般在生成阶段用细 ds保证几何精度检测阶段先对离散点做粗过滤再用细检测。4.3 退化边界距离恰好卡在临界值附近Dubins 路径有几个临界距离它们边界处的几何形态会发生突变。第一个是 d 2ρ。此时 LSR / RSL 的内切线退化为一个点两段圆弧相切直线段长度为零路径变成两段圆弧直接相交。公式上 asin(2ρ/d) 在 d 2ρ 时等于 asin(1) π/2切线方向和径向方向垂直一切正常。但如果 d 略小于 2ρasin 的参数大于 1MATLAB 会返回复数结果路径变成 NaN。处理方式是在内切线函数入口做距离判断距离不足时直接返回一个很大的长度值让主函数自动避开这个模式。第二个是 d 4ρ。CCC 模式的中间圆圆心与两端圆心连线中点重合h 0三段圆弧的衔接点在同一直线上中间圆弧转角接近 π。浮点误差可能让根号里的值变成微小的负数所以 h 的计算必须套 max(0, ...)。此时路径几何成立但状态很脆稍微扰动终点位置最优模式就可能从 CCC 跳到 CSC。如果应用场景里经常出现这种临界距离建议在规划层留一个后手距离过近时直接让车辆先走一段圆弧绕开而不是硬套 Dubins 公式。第三个退化和角度有关。当起点航向与终点航向之差接近 π 时LSL 或 RSR 的首段圆弧转角可能接近 2π路径会长出几乎一整圈。这不算错误但会显著增大路径长度。排查时如果发现最优路径的长度接近 2πρ说明起点终点朝向几乎相反可以考虑反向规划一次终点状态加 π 再规划对比长度后取短。4.4 不同平台的适用差异Dubins 曲线最初是为固定翼飞行器设计的后来被大量用在轮式车辆上但两类平台对路径的要求差别很大。固定翼不能倒退Dubins 的假设始终前进完全成立路径可以直接送飞控。轮式车辆如果允许倒车Reeds-Shepp 曲线更合适——它扩展了倒车段路径更短但切换档位会增加控制复杂度。自动泊车场景中Dubins 曲线最常被当作混合 A* 的启发项。混合 A* 在栅格上搜索时用 Dubins 路径长度作为当前节点到终点的启发代价能明显加速搜索收敛。此时 ρ 直接取车辆最小转弯半径不需要额外调参但要注意启发项必须满足一致性admissible否则搜索会丢失最优解。这些工程细节说明一个道理Dubins 曲线是基础路径语义选型和参数都要围绕你要解决的实际运动约束来做。5. 常见问题与排查Dubins 路径落地的五个坑5.1 圆弧方向算反路径绕了一个大圈现象生成的 LSL 路径不是流畅的 S 形而是先绕了几乎一整圈才接到直线段路径长度明显偏大甚至看起来像在故意兜圈子。原因圆弧方向符号和圆心位置不一致。比如左转圆弧的圆心位置用了 th - π/2 计算但圆弧采样时又按逆时针转两者叠加后实际轨迹方向完全相反尤其在航向角为负时符号错误会被放大。解决把圆心位置和转向方向捆绑验证。测试时用一个最简单的状态起点 (0, 0)、航向 0令弧段转角为 π/2输出点列应落在第四象限到第一象限的逆时针四分之一圆上。把这个测试固化成单元测试函数每次改动几何代码后先跑一遍比肉眼读路径可靠得多。5.2 atan2 和角度回绕打架拼接点错位现象起点圆弧和直线段的连接点处出现一个明显的折角或者直线段起点的坐标和圆弧终点坐标有微小偏差路径在拼接处断开几毫米。原因多个 atan2 计算得到的角度在 ±π 附近回绕时直接相减得到的结果可能落在 (-2π, 0) 或 (0, 2π) 区间导致转角方向判断错误。wrap_angle 函数单独使用时没问题但在「先计算圆心角差、再叠加转向方向」的组合运算里回绕时机不对就会出错。解决统一约定——所有角度差值先折算到 (-π, π]再根据转向方向取正负号。不要在中间步骤里保留未回绕的大角度。调试时把每个拼接点的坐标、航向、圆心角都打印出来人工核对一遍通常一眼就能看到是哪个环节的角度差算错了。5.3 内切线在 d 2ρ 时返回 NaN现象调用 dubins_path 时LSR 或 RSL 模式内部报错路径点全是 NaN程序中断。原因asin(2ρ/d) 在 d 2ρ 时无实数解这是几何上的硬约束不是数值 bug。有些实现没做检查就把复数传给了后续的 cos / sinMATLAB 不报错但输出 NaN排查时更隐蔽。解决入口处显式判断d 2ρ 时返回一个很大的长度比如 1e12让主函数自然跳过该模式。同时要注意如果六种模式全部因为几何约束失效说明起点终点状态根本不满足可行性条件工程上需要给更上层的规划模块回传一个失败标志而不是静默返回空路径。我在模拟项目 X 里遇到过这种场景终点在障碍物后面很近的位置所有 Dubins 模式都不可达后来通过引入一个中间航点才解决。5.4 切点坐标正确但拼接处航向不连续现象路径在图上看着连续但把每个离散点的 th 提取出来画成曲线后发现某个拼接点两侧的航向差了 90 度甚至 180 度。控制器跟踪到该点时会突然急转。原因圆弧段的航向计算依赖「径向角 dir × π/2」这个公式如果 dir 取值与圆心位置不一致或者 wrap_angle 在 π 边界回绕错误航向角就会跳变到对侧。还有一种情况内切线的两个切点分别落在两个圆的相反侧几何上正确但车辆通过直线段到达切点 2 时的航向和终点圆弧在切点 2 处的切向方向不一致——这在数学上不应该发生一旦出现就说明切点计算和圆弧角度计算用了不同的坐标系基准。解决写一个专门的连续性检查函数遍历所有拼接点比较相邻两段在连接点处的航向差超过阈值比如 1 度就告警。规则很简单但能省下大量肉眼排查的时间。5.5 把 Dubins 曲线直接当避障路径用现象路径生成成功也能跟踪但车辆撞上了障碍物——因为 Dubins 曲线根本没有障碍物信息它只保证满足转弯半径约束。原因这是使用场景的错位。Dubins 是「两状态之间的最短可行路径」不含任何环境建模。有些初学者把它当成全局规划器以为输出天然安全。解决正确的工程链路是全局规划器A* / RRT / 混合 A*负责避障并给出航点序列Dubins 曲线负责连接相邻航点或作为搜索启发项。航点设计时还要注意两个航点之间的距离不能太近否则 Dubins 路径可能绕圈或退化一般保持 3ρ 以上的间隔较稳。碰撞检测可以放在 Dubins 路径生成之后做对离散点逐一查障碍物距离碰撞则丢弃该段或调整航点位置重规划。6. 进阶落地从单段曲线到完整路径规划的几个小技巧先验证再使用是我对 Dubins 路径代码的基本要求。每次改完几何计算我都会跑一个曲率检查对输出的离散点序列计算实际曲率确认没有超过 1/ρ。% 曲率检查: 对离散路径点计算实际曲率 ds_actual sqrt(diff(x).^2 diff(y).^2); s_cum cumsum([0, ds_actual]); kappa_est diff(wrap_angle(diff(th))) ./ diff(s_cum(2:end)); if max(abs(kappa_est)) 1/rho 1e-6 warning(曲率超限, 路径不可执行); end这段代码用相邻航向角变化量除以弧长来估计曲率。注意用法diff(th) 先取相邻航向差再用 wrap_angle 回绕避免航向从接近 π 跳到接近 -π 时产生虚假的大曲率。这个检查本质上是把 Dubins 曲线当黑匣子时的最后一道防线能抓住绝大多数几何符号错误。第二个技巧是把 Dubins 路径嵌入到混合 A* 框架里做启发项。搜索时每个栅格节点存状态 (x, y, θ)以节点为起点、目标状态为终点调用 dubins_path把返回的解析长度作为该节点的启发代价。由于启发值是基于运动学约束算出来的比欧氏距离紧得多搜索效率提升明显。这一步不需要修改 Dubins 核心代码只需要把主函数的返回值和输入接口封装成独立函数。第三个技巧是起点横向偏差修正。实际车辆规划时GPS 或里程计给出的起点航向和真实航向之间通常有几度的误差导致 Dubins 路径的起始段和真实轨迹产生横向偏差。一个简单实用的做法是在起点后加一段过渡圆弧圆弧半径大于 ρ把车辆从「带偏差的起始状态」平滑过渡到「理想起始状态」。过渡圆弧的圆心和角度用数值优化搜索确定目标函数是过渡结束后到原路径的横向偏差最小。这个方法比直接增大 ρ 更可控因为过渡段半径可以独立调节。最后说一个踩过的坑多段 Dubins 路径拼接时上一段的终点和下一段的起点状态完全一致但曲率方向可能相反一段右转、一段左转。直接把两段离散点拼接车辆在连接点处要瞬间从右转切换成左转转向机构根本反应不过来。解决办法是在拼接点插入一个回环过度段或者干脆让全局规划器保证航点之间的朝向变化不会太大。后来我养成的习惯是所有 Dubins 路径输出前都跑一次「段间连续性检查」包含位置、航向、曲率三个维度任何一个超限就标记为不可用。这套流程看着繁琐但每次部署到新平台时它都能在实车测试前拦住大半路径质量问题。希望这些经验帮你在 Dubins curve 的 MATLAB 实现路上少走几个弯路。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
