简介：这份资源是面向路径规划与航迹规划方向的学习者与开发者的MATLAB实现包，聚焦间接PH曲线（Indirect PH Curves）在二维空间中的路径设计与优化。内容围绕PH曲线构建、曲率计算、Bezier曲线求导与de Casteljau算法展开，并包含PH曲线与Bezier曲线的对比实验、曲线偏移与相交处理等模块，适合具备一定数值计算与机器人运动规划基础的研究者参考。压缩包共45个文件，以18个m脚本文件为核心，配合14个fig图形文件与12个png结果图，另有1个README说明文档，整体约615KB，便于直接运行与结果复现。目前已有241人学习下载。通过源码与配套图示，读者可理解间接PH曲线的生成流程、曲率与偏移曲线的实现细节，并借助对比脚本与实验图快速验证算法效果，为路径平滑、避障与航迹优化提供可复用的代码参考。

1. 从 Indirect-PH-curves 说起：为什么航迹规划里 PH 曲线值得单独拎出来

如果你做过无人机三维路径规划、AGV 调度或者喷漆机器人的轨迹生成，大概率遇到过同一个尴尬：规划器吐出来的是一串离散航点，交给控制器之前得先拟合成光滑曲线，而拟合出来的曲线要么曲率跳变，要么弧长算不准，速度规划直接崩掉。Indirect-PH-curves 这个方向，解决的就是这件事——用 PH 曲线（Pythagorean Hodograph，勾股速端曲线）做航迹规划，让曲线本身自带「弧长是多项式」这个性质，速度规划和实时插值都变得干净。它适合已经跑通 A*、RRT 这类离散搜索、但卡在后端平滑与速度分配的从业者，也适合做泊车路径规划、喷漆路径规划这类对曲率连续性有硬要求的场景。PH 曲线不是新概念，但把它和间接法（Indirect）结合做航迹生成，是近几年在无人机路径规划和 AGV 路径规划里被反复提起的落地路线。

2. PH 曲线的数学底子与 Indirect 构造：为什么它比 B 样条更适合做航迹

2.1 PH 曲线的定义与弧长多项式性质

普通参数曲线 r(t) = (x(t), y(t))，其导数 r'(t) 的模长一般不是多项式，所以弧长 s(t) = ∫|r'(t)|dt 没有解析表达式，只能数值积分。PH 曲线的定义就是给这个模长加一个约束：存在多项式 σ(t)，使得

x'(t)² + y'(t)² = σ(t)²

也就是说，速端曲线的模长本身是一个多项式。这个约束带来的直接好处是弧长 s(t) = ∫σ(t)dt 是多项式，可以精确计算，不需要数值积分。对航迹规划来说，这意味着速度规划里的时间-弧长映射可以解析求解，T 型或 S 型速度曲线能直接套公式，不用迭代。

PH 曲线的构造方式有两类：一类是直接法，先给定控制点再反解；另一类是间接法（Indirect），先给定速端曲线的多项式形式，再积分得到曲线。Indirect 构造的典型形式是令

x'(t) = u(t)² - v(t)² y'(t) = 2u(t)v(t)

其中 u(t)、v(t) 是低阶多项式。这样自动满足 PH 条件，σ(t) = u(t)² + v(t)²。积分一次就得到 x(t)、y(t)。这种构造的好处是曲线次数可控，u、v 取一次多项式时得到三次 PH 曲线，取二次时得到五次 PH 曲线。三次 PH 曲线在无人机航迹里用得最多，因为它的曲率连续，且自由度刚好够做 G1 或 G2 拼接。

2.2 Indirect 构造的步骤与参数选择

用 Indirect 法生成一条 PH 曲线，实际步骤是这样的：

第一步，确定端点条件。航迹规划里通常给定起点 P0、终点 P1，以及两端的切向角 θ0、θ1。如果是 G2 拼接，还要给定两端曲率 κ0、κ1。

第二步，选择 u(t)、v(t) 的次数。三次 PH 曲线对应 u、v 均为一次多项式：

u(t) = u0 + u1·t v(t) = v0 + v1·t

第三步，把端点条件代入积分后的曲线表达式，得到一个非线性方程组。未知量是 u0、u1、v0、v1 以及曲线参数总长对应的 t1。方程个数取决于约束类型：G1 拼接给 4 个方程（两端位置 2 个、两端切向 2 个），G2 拼接给 6 个方程。

第四步，解这个方程组。常用做法是牛顿-拉夫逊迭代，初值取直线连接时的参数。这里有个血泪经验：初值选得不好，迭代会收敛到自交曲线或者曲率符号翻转的解，所以一般会加一个曲率符号约束做筛选。

下面是一段用 Python 做三次 PH 曲线 Indirect 构造的最小示例，依赖 numpy 和 scipy：

import numpy as np from scipy.optimize import fsolve def ph_curve_3rd(P0, P1, theta0, theta1): """ 三次 PH 曲线 Indirect 构造 P0, P1: 起点终点 (x, y) theta0, theta1: 两端切向角 (弧度) 返回: 曲线采样点、弧长多项式系数 """ def equations(vars): u0, u1, v0, v1, t1 = vars # 速端曲线 def r_prime(t): u = u0 + u1 * t v = v0 + v1 * t return np.array([u**2 - v**2, 2*u*v]) # 数值积分得到端点位移 ts = np.linspace(0, t1, 200) dt = ts[1] - ts[0] dx = sum(r_prime(t)[0] * dt for t in ts) dy = sum(r_prime(t)[1] * dt for t in ts) # 端点切向约束 rp0 = r_prime(0) rp1 = r_prime(t1) ang0 = np.arctan2(rp0[1], rp0[0]) ang1 = np.arctan2(rp1[1], rp1[0]) return [ dx - (P1[0] - P0[0]), dy - (P1[1] - P0[1]), ang0 - theta0, ang1 - theta1, u0 - 1.0 # 固定尺度，避免无穷多解 ] # 初值：直线连接 L = np.linalg.norm(np.array(P1) - np.array(P0)) guess = [np.sqrt(L), 0.1, 0.1, 0.1, 1.0] sol = fsolve(equations, guess, full_output=True) u0, u1, v0, v1, t1 = sol[0] # 采样 ts = np.linspace(0, t1, 100) pts = [] x, y = P0 for i in range(1, len(ts)): dt = ts[i] - ts[i-1] u = u0 + u1 * ts[i] v = v0 + v1 * ts[i] x += (u**2 - v**2) * dt y += 2 * u * v * dt pts.append([x, y]) return np.array(pts), (u0, u1, v0, v1, t1) # 示例：从 (0,0) 到 (10,5)，切向 0 和 pi/4 pts, params = ph_curve_3rd((0,0), (10,5), 0, np.pi/4) print("采样点数:", len(pts)) print("参数:", params)

这段代码的逻辑说明：equations 函数里把端点位移、切向角、尺度固定写成 5 个残差方程，fsolve 做牛顿迭代。参数说明：u0、u1、v0、v1 是速端多项式系数，t1 是参数终点。注意这里用数值积分近似端点位移，实际工程里应该用解析积分，因为 PH 曲线的弧长是多项式，解析积分更快也更准。另外固定 u0 = 1.0 是为了消除尺度自由度，否则方程组欠定。

2.3 为什么航迹规划偏爱 PH 曲线而不是 B 样条

B 样条也能做光滑航迹，而且局部支撑性好，但它在航迹规划里有三个硬伤。第一，弧长没有解析表达式，速度规划必须数值积分，实时性差。第二，曲率计算涉及一阶和二阶导数的组合，数值稳定性在接近直线段时容易出问题。第三，B 样条的控制点不直接对应几何约束，做曲率上限约束时需要额外优化。

PH 曲线的优势正好补上这三点：弧长多项式、曲率有解析表达式、Indirect 构造可以直接把端点切向和曲率作为约束代入。在无人机三维路径规划里，常见做法是把三维问题拆成水平面和高度两个 PH 曲线，或者用空间 PH 曲线的四元数形式。AGV 路径规划里更多用平面三次 PH 曲线做 G1 拼接，泊车路径规划则用五次 PH 曲线做 G2 拼接，因为泊车对曲率连续要求更高。

提示：三次 PH 曲线能保证曲率连续，但曲率变化率不连续。如果控制器对 jerk 敏感，比如喷漆机器人，建议直接用五次 PH 曲线，代价是方程组从 5 个未知量变成 7 个，迭代初值更难选。

3. 把 PH 曲线接进航迹规划流水线：从离散航点到可执行轨迹

3.1 前端离散搜索与后端 PH 平滑的接口设计

实际航迹规划流水线通常是两段式：前端用 A*、RRT* 或者 Hybrid A* 搜出一串离散航点，后端用 PH 曲线做平滑和速度分配。接口设计的关键是决定哪些航点作为 PH 曲线的端点。常见做法是：前端搜索时保留拐点，后端在每两个拐点之间生成一条 PH 曲线，拼接处保证 G1 或 G2 连续。

具体步骤：

1. 前端搜索输出航点序列 W = {w0, w1, ..., wn}，每个航点带位置和朝向。
2. 计算每个航点的转向角，超过阈值的标记为拐点。
3. 在相邻拐点之间调用 PH 曲线生成，端点切向取拐点处的朝向。
4. 检查拼接处曲率是否连续，不连续则调整拐点位置或改用 G2 拼接。
5. 对整条 PH 曲线做速度规划，利用弧长多项式直接算时间-弧长映射。

这里有个容易翻车的地方：前端搜索的航点朝向如果不准，PH 曲线会扭得很厉害。我一般会在前端加一个朝向平滑，用滑动平均或者最小二乘拟合先粗修一遍，再交给 PH 曲线。

3.2 曲率约束与避障的联合处理

PH 曲线本身不处理避障，避障要在前端搜索或者后端优化里做。常见做法是：前端搜索时把障碍物膨胀，保证航点不撞；后端 PH 曲线生成后，检查曲线到障碍物的最近距离，如果小于安全阈值，就在该段插入中间航点重新生成。

曲率约束的处理更直接：三次 PH 曲线的曲率有解析表达式

κ(t) = 2(u v' - u' v) / (u² + v²)²

其中 u、v 是速端多项式。给定曲率上限 κ_max，可以在方程组里加不等式约束，或者生成后检查，超限就调整端点切向。实际工程里更常用后者，因为加不等式约束会让牛顿迭代变得复杂。

下面是一个曲率检查的代码片段：

def check_curvature(u0, u1, v0, v1, t1, kappa_max): """ 检查三次 PH 曲线曲率是否超限 返回: 最大曲率、超限位置列表 """ ts = np.linspace(0, t1, 500) max_kappa = 0 violations = [] for t in ts: u = u0 + u1 * t v = v0 + v1 * t u_prime = u1 v_prime = v1 # 曲率公式 numerator = 2 * (u * v_prime - u_prime * v) denominator = (u**2 + v**2)**2 kappa = abs(numerator / denominator) if denominator > 1e-12 else 0 if kappa > max_kappa: max_kappa = kappa if kappa > kappa_max: violations.append((t, kappa)) return max_kappa, violations # 接上面的参数 max_k, viol = check_curvature(*params, kappa_max=0.5) print("最大曲率:", max_k) print("超限点数:", len(viol))

逻辑说明：曲率公式来自 PH 曲线的参数表达式，分母是速端模长的四次方。参数说明：kappa_max 根据车辆或无人机的转向能力设定，AGV 一般取 0.5~1.0，无人机可以放宽到 2.0。如果超限点多，说明端点切向需要调整，或者该段应该拆成多条 PH 曲线。

3.3 速度规划：用弧长多项式直接算时间映射

PH 曲线最大的落地价值在速度规划。因为弧长 s(t) 是多项式，给定速度曲线 v(s)，时间 t 可以通过积分直接算：

t(s) = ∫ ds / v(s)

如果 v(s) 是分段常数（T 型速度），t(s) 就是分段线性，直接套公式。如果 v(s) 是梯形或 S 型，也能解析积分。这比 B 样条的数值积分快一个数量级，在嵌入式控制器上差别很明显。

实际做法是：先根据曲率约束算最大允许速度 v_max(s) = sqrt(a_lat_max / κ(s))，再和全局速度上限取 min，得到速度剖面。然后用弧长多项式算每个控制周期对应的参数 t，插值出位置和速度发给控制器。

注意：曲率在 PH 曲线端点可能趋近于零，导致 v_max 算出来无穷大。实际代码里要加一个速度上限截断，否则速度规划会输出离谱值。

4. 避坑与排查：PH 曲线航迹规划里最容易翻车的 5 个点

4.1 现象：牛顿迭代不收敛，曲线自交

原因：初值选得太远，或者端点切向差超过 180 度。PH 曲线的 Indirect 构造方程组是非线性的，解不唯一，迭代容易跑到自交解。

解决：初值用直线连接的参数，切向差超过 180 度时先拆成两段。另外加一个自交检测，用曲线采样点做线段相交判断，发现自交就换初值重试。

4.2 现象：拼接处曲率跳变，控制器抖动

原因：两条 PH 曲线在拼接点只保证了 G1 连续，曲率不连续。三次 PH 曲线做 G1 拼接时，曲率由端点切向和速端多项式决定，两边独立生成时曲率对不上。

解决：改用 G2 拼接，把拼接点的曲率作为约束代入两条曲线的方程组。或者用五次 PH 曲线，它的自由度足够同时满足位置、切向、曲率连续。

4.3 现象：弧长算出来和实际走的不一样

原因：PH 曲线的弧长多项式是在参数域上精确的，但实际控制器走的是离散插值，如果插值步长太大，弧长会有累积误差。

解决：插值步长按曲率自适应，曲率大的地方步长小。一般取步长使得每步弧长不超过 0.01 米，或者参数增量不超过 0.01。

4.4 现象：避障检查通过但实际撞了

原因：PH 曲线生成后只检查了采样点到障碍物的距离，采样点太稀，曲线在采样点之间鼓出去撞到障碍物。

解决：避障检查用曲线到障碍物的解析距离，或者加密采样。对圆形障碍物，可以算曲线到圆心的最小距离，用优化方法求极值，比采样更可靠。

4.5 现象：速度规划在曲率突变处超速

原因：曲率约束算 v_max 时用了采样点的曲率，采样点之间的曲率峰值被漏掉。

解决：曲率峰值可以用解析表达式求导找极值点，或者用自适应采样。实际工程里更简单：在曲率约束上乘一个安全系数，一般取 0.8。

5. 进阶技巧：用 PH 曲线做动态避障与多段拼接的验证方法

动态避障场景下，PH 曲线不能只生成一次。常见做法是滚动时域：每个控制周期用当前位姿和局部目标重新生成一条 PH 曲线，只取前一小段执行。这样避障响应快，但拼接频繁，对 G2 连续性要求更高。我一般会用五次 PH 曲线做滚动拼接，拼接点曲率约束用上一段末端的曲率。

多段拼接的验证方法，我习惯用三个指标：拼接点曲率误差、整条航迹的最大曲率、弧长积分误差。曲率误差直接算两条曲线在拼接点的 κ 值差，一般要求小于 0.01。最大曲率用解析表达式扫描，确认不超限。弧长积分误差用解析弧长和数值积分对比，误差应该在 1e-6 量级。

下面是一个多段拼接验证的代码框架：

def validate_ph_chain(segments, kappa_max=0.5): """ 验证多段 PH 曲线拼接 segments: 每段的 (u0, u1, v0, v1, t1, P_start) 返回: 验证报告 """ report = {"curvature_jumps": [], "max_kappa": 0, "arc_errors": []} prev_end_kappa = None for i, seg in enumerate(segments): u0, u1, v0, v1, t1, P0 = seg # 末端曲率 u = u0 + u1 * t1 v = v0 + v1 * t1 kappa_end = abs(2 * (u * v1 - u1 * v) / (u**2 + v**2)**2) # 拼接点曲率跳变 if prev_end_kappa is not None: jump = abs(kappa_end - prev_end_kappa) report["curvature_jumps"].append((i, jump)) prev_end_kappa = kappa_end # 最大曲率 max_k, _ = check_curvature(u0, u1, v0, v1, t1, kappa_max) report["max_kappa"] = max(report["max_kappa"], max_k) # 弧长误差：解析 vs 数值 # 解析弧长 = ∫(u²+v²)dt，数值用采样 ts = np.linspace(0, t1, 1000) arc_analytic = sum((u0 + u1*t)**2 + (v0 + v1*t)**2 for t in ts) * (t1/1000) arc_numeric = sum(np.sqrt((u0+u1*t)**2 + (v0+v1*t)**2) for t in ts) * (t1/1000) report["arc_errors"].append(abs(arc_analytic - arc_numeric)) return report

逻辑说明：这个框架逐段检查曲率跳变、最大曲率和弧长误差。参数说明：kappa_max 是全局曲率上限，segments 里每段带起始点。注意弧长解析式是 ∫σ(t)dt = ∫(u²+v²)dt，数值式是 ∫sqrt(x'²+y'²)dt，两者在 PH 条件下应该相等，误差反映数值积分精度。

我自己的习惯是：每次改完 PH 曲线参数，先跑这个验证，确认三个指标都过再上控制器。翻车最多的就是曲率跳变，尤其是从直线段切到曲线段的时候，拼接点曲率从 0 跳到 0.3 以上，控制器直接抖给你看。后来我固定用五次 PH 曲线做所有拼接，曲率跳变控制在 0.005 以内，这个问题就没再出现过。希望帮到你。

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