4. 航路点几何与路径生成

航路点任务只给出了“必须经过哪些位置”，但AUV真正执行时需要一条连续、可微、可跟踪的期望路径。把离散的航路点序列变成期望路径，就是路径生成。它决定了AUV在拐角处是急转、停顿还是平滑绕行，也直接决定了跟踪控制器需要付出多大的操舵代价。本节围绕三种最常用的生成方式——直线段、圆弧过渡、样条曲线——说明它们的几何构造、参数选择以及对运动平滑性和跟踪精度的影响。

一、直线段路径：最直接的“折线”

最朴素的生成方式是依次连接相邻航路点，得到一条折线路径。设航路点序列为 P₀, P₁, …, P_n，第 i 段路径为从 P_i-1 到 P_i 的线段。对AUV而言，折线路径在拐点处方向发生突变，期望航向角不连续。若AUV以速度 v 沿路径运动，在拐点处需要瞬时改变航向，实际执行中表现为减速、超调或绕大弯，跟踪误差在拐角附近明显增大。

折线路径的优点是实现简单、几何明确，适合开阔水域的粗略引导或作为其他方法的基准。工程上常用“到线段的有符号横向偏差”作为跟踪误差：对第 i 段，设线段方向单位向量为 t，法向单位向量为 n（取左侧为正），AUV位置为 P，则横向偏差 e = (P − P_i-1)·n。控制器据此修正航向。但折线本身不解决拐角平滑问题，因此通常需要配合“到点切换”逻辑：当AUV进入以 P_i 为圆心、半径 R_accept 的接受圆时，切换到下一段。R_accept 过小会导致切段滞后、绕圈；过大则会提前切段、切角。示例中可取 R_accept = 1.5～2 倍AUV回转半径作为起点，再根据实际航迹调整。

二、圆弧过渡：用“倒角”换取平滑

圆弧过渡的思路是在相邻两段直线的拐角处插入一段圆弧，使路径在切点处与两段直线相切，从而保证航向连续。设拐角顶点为 P_i，前后两段方向夹角为 θ（即转向角），选定过渡半径 r，则切点到顶点的距离 d = r·tan(θ/2)。圆弧圆心位于角平分线上，距顶点距离为 r / cos(θ/2)。这样，路径由“直线—圆弧—直线”组成，航向连续但曲率在切点处仍不连续（从0跳到1/r），属于 C1 连续。

圆弧过渡的关键参数是 r。r 越大，拐角越平缓，但切点越远离顶点，可能“吃掉”相邻航段的可用长度；当相邻航段较短时，两端的过渡圆弧可能重叠，此时需要缩小 r 或改用其他方法。工程上常约束 d ≤ 0.5·min(前段长度, 后段长度)。对AUV而言，r 还应与最小回转半径匹配：若 r 小于AUV物理上能实现的最小转弯半径，控制器将无法贴合圆弧，产生持续横向误差。示例中，若AUV在巡航速度下的最小回转半径约 8 m，则可取 r = 10～15 m，并检查各拐角处 d 是否满足上述约束。

圆弧过渡的另一个实用细节是转向方向判定。用前后两段方向向量的叉积符号判断左转或右转，确保圆弧圆心在正确一侧；若符号判断错误，圆弧会向反方向鼓出，路径将穿过障碍或偏离预期。这一点在实现时必须与坐标系约定一致：本文统一采用右手系、航向角从北向顺时针为正、左侧为正横向偏差。

三、样条曲线：全局光滑的路径

当航路点较密、希望整条路径具有连续曲率时，可采用样条曲线。常用的是三次样条或B样条：以航路点为控制点（或插值点），生成一条经过或逼近这些点的光滑曲线。三次样条保证位置、速度、加速度连续，即 C2 连续，曲率变化平缓，非常适合AUV的平滑跟踪。B样条则具有局部支撑性，调整一个控制点只影响局部路径，便于在线修改。

样条的代价是路径不再严格经过中间航路点（B样条情形），或虽然经过但可能产生过冲（三次样条在点距不均时容易出现）。因此使用样条时需关注两个问题：一是参数化方式，按弦长参数化通常比均匀参数化更稳定；二是曲率检查，生成后应计算各点曲率 κ，确认最大曲率对应的转弯半径 1/κ_max 不小于AUV最小回转半径，否则需增加航路点密度或调整控制点。示例中可在生成后以 0.5 m 步长离散采样，输出曲率序列，若发现某处 κ 超过 1/8 m⁻¹（即半径小于 8 m），则在该区域插入一个辅助航路点重新生成。

四、三种方式对平滑性与跟踪精度的影响

从几何连续性看，折线只有位置连续（C0），圆弧过渡达到航向连续（C1），样条可达到曲率连续（C2）。连续性越高，期望路径的曲率变化越平缓，AUV的舵角指令和横向加速度越平滑，跟踪时的稳态误差和超调通常越小。但平滑性并非越高越好：样条在航路点稀疏时可能偏离原始航路点较远，若这些点靠近障碍或狭窄水道，偏离会带来安全风险；圆弧过渡则会“切角”，在必须严格经过航路点的任务中需要评估切角量是否可接受。

跟踪精度还取决于路径与控制器是否匹配。折线路径配合纯追踪或视线法时，在拐角处容易产生较大横向误差；圆弧过渡配合曲率前馈控制效果较好，因为圆弧段曲率已知且恒定；样条路径则需要控制器能够处理时变曲率，通常要求更高的控制带宽。实际工程中常见的折中方案是：远距离航段用直线，拐角用圆弧过渡，仅在需要精细避障或地形跟随的局部区域使用样条。

五、一个可执行的生成与检查流程

下面给出一个与本节内容配套的路径生成检查流程，可直接用于实现或复核。输入为航路点列表（含坐标）、AUV最小回转半径 R_min、接受圆半径 R_accept；输出为离散化的期望路径点列及曲率检查结果。

输入: waypoints = [P0, P1, ..., Pn]  (单位: m)
      R_min = 8.0        # AUV最小回转半径
      R_accept = 12.0    # 到点接受圆半径
      ds = 0.5           # 路径离散步长

步骤1  对每对相邻航路点计算段长 L_i 与方向角 psi_i
步骤2  对每个中间点 Pi 计算转向角 theta_i = psi_{i+1} - psi_i
步骤3  若采用圆弧过渡:
         r_i = min(1.5*R_min, 0.5*min(L_i, L_{i+1}) / tan(|theta_i|/2))
         d_i = r_i * tan(|theta_i|/2)
         检查 d_i <= 0.5*min(L_i, L_{i+1})，否则缩小 r_i
步骤4  按直线-圆弧-直线拼接，沿路径以 ds 采样
步骤5  计算采样点曲率 kappa，检查 1/max(kappa) >= R_min
步骤6  输出路径点列、各拐角 r_i、最大曲率及对应位置

该流程中，步骤3的半径约束同时考虑了物理回转能力和相邻航段长度，避免圆弧重叠；步骤5的曲率检查是判断路径是否“可跟踪”的硬指标。若检查不通过，优先在该拐角附近增加一个辅助航路点，把大转向角拆成两个小转向角，通常能显著降低最大曲率。示例参数下，若某拐角 θ = 90°、相邻段长均为 40 m，则 r 取 12 m 时 d ≈ 12 m，满足约束；若相邻段长仅 15 m，则 d 会超过可用长度的一半，需要把 r 降到约 7.5 m 以下，此时应复核该半径是否小于 R_min，若小于则说明该拐角在几何上无法用单段圆弧平滑通过，应改用样条或增加航路点。

路径生成完成后，建议把期望路径与AUV实际航迹叠加显示，重点观察拐角处的横向偏差峰值和切段时刻。若偏差峰值集中在圆弧段入口，通常是过渡半径偏小或控制器前馈不足；若偏差出现在直线段中部，则更可能是航向对齐或侧向流干扰问题，而非路径生成本身。通过这种“几何参数—曲率—跟踪误差”的对应关系，可以逐步把路径生成从凭经验试凑变成有依据的设计。