企业官网建设流程全解析

简介：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 曲线的最优性证明给出了一个关键结论：任意两点之间的最短路径，只会出现在以下六种模式中。

这六种模式分为两大类。前四种属于 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) % 半径相等两圆的内切线切点, side=+1/-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 路径里的 ρ 不是随便取的，它代表车辆或飞行器实际能执行的最小转弯半径。对于前轮转向车辆，最小转弯半径通常从车辆手册里查，或者直接实测：把方向盘打死，绕一圈量直径。对于差速底盘，最小转弯半径理论上可以为零（原地转），但实际运动学里原地转会导致轮胎磨损和定位漂移，工程上一般设一个大于零的下限。

数值下限这一层很容易被忽视。如果 ρ 设成 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 的参数大于 1，MATLAB 会返回复数结果，路径变成 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 / sin，MATLAB 不报错但输出 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 实现路上少走几个弯路。

本文还有配套的精品资源，点击获取