3. 路径参数化与跟踪误差定义

在AUV路径跟踪中，我们真正关心的不是“船现在在哪”，而是“船相对于期望路径偏了多少、朝哪个方向偏”。要把这个问题说清楚，第一步是把期望路径用数学方式表达出来，第二步是定义一套与路径绑定的误差坐标。本节围绕这两件事展开：先看直线、圆弧、样条三类路径的参数化写法，再定义横向跟踪误差与航向误差，最后推导误差动态方程，为后续LOS制导律设计打基础。

一、路径参数化：把几何曲线写成参数方程

路径参数化的本质，是引入一个标量参数 s（通常取弧长或归一化弧长），把路径上任意一点表示成 P(s) = [x_p(s), y_p(s)]。这样，路径上每一点都有唯一的参数坐标，AUV在路径上的“投影点”也就有了明确的数学含义。工程上最常用的三类路径如下。

直线路径最简单。设起点为 P0=(x0,y0)，单位方向向量为 t=(cosα, sinα)，则

x_p(s) = x0 + s·cosα
y_p(s) = y0 + s·sinα,   s ∈ [0, L]

其中 L 为直线段长度，α 为直线相对大地坐标系（北东地，x轴指北、y轴指东）的方位角。直线路径的切向量处处相同，曲率为零，是最容易分析的基准情形。

圆弧路径用圆心 C=(xc,yc)、半径 R 和起始方位角 θ0 描述。以圆心角 θ 为参数：

x_p(θ) = xc + R·cos(θ0 + σ·θ)
y_p(θ) = yc + R·sin(θ0 + σ·θ),   θ ∈ [0, Θ]

这里 σ=+1 表示逆时针（左转），σ=-1 表示顺时针（右转），Θ 为圆弧总圆心角。若改用弧长参数 s，则 θ = s/R，切向量随 s 连续旋转，曲率恒为 1/R。圆弧是检验制导律在常曲率路径上稳态性能的标准工况。

样条路径用于连接多个航路点并保证曲率连续。工程上常用三次样条或B样条：给定航路点 W0…Wn，分段构造 x_p(u)、y_p(u)，u∈[0,1] 为段内归一化参数。以三次样条为例，每段满足端点位置与一阶、二阶导数连续，从而避免曲率突变引起的航向指令跳变。需要注意：样条参数 u 一般不等于弧长 s，实际投影计算时通常先按 u 粗定位，再用弧长积分或牛顿迭代修正到最近点。

二、横向跟踪误差与航向误差的定义

定义误差前必须先约定坐标系，全文保持一致：采用北东地惯性系 {I}，AUV位置为 (x,y)，航向角 ψ 从x轴（北）起算，逆时针为正。路径在投影点处的切向角记为 ψ_p。

横向跟踪误差 e 定义为AUV位置到路径投影点的有符号法向距离。设投影点处单位切向量为 t_p，单位法向量 n_p 取 t_p 逆时针旋转90°，则

e = n_p · (P_auv − P_p)

按此约定，AUV位于路径左侧时 e>0，右侧时 e<0。这个符号约定直接影响后续制导律中航向修正项的符号，必须与控制器实现严格一致。

航向误差定义为AUV航向与路径切向之差：

ψ_e = ψ − ψ_p

并归一化到 (−π, π]。当 ψ_e=0 且 e=0 时，AUV恰好沿路径前进，这是路径跟踪的平衡状态。

三、误差动态方程推导

设AUV航速为 U，航向角速度为 r，并假设AUV做协调转弯、无显著侧滑，则运动学为 ẋ=U·cosψ、ẏ=U·sinψ、ψ̇=r。对 e 求导，关键在于投影点沿路径以速度 ṡ 移动，且路径切向角以 ψ̇_p = κ·ṡ 变化（κ 为投影点处曲率）。经几何分解可得

ė = U·sin(ψ_e)
ψ̇_e = r − κ·ṡ

其中 ṡ = U·cos(ψ_e) / (1 − κ·e)。当 |κ·e| ≪ 1 时，可近似 ṡ ≈ U·cos(ψ_e)，于是

ė ≈ U·sin(ψ_e)
ψ̇_e ≈ r − κ·U·cos(ψ_e)

这两个式子揭示了路径跟踪的核心耦合：横向误差的变化率由航向误差的正弦驱动，而航向误差又受AUV转艏能力和路径曲率共同影响。对直线路径 κ=0，第二式退化为 ψ̇_e=r，误差系统成为典型的非完整约束链式系统；对圆弧路径，κ·U 项相当于一个持续存在的“曲率扰动”，制导律必须提供相应的前馈补偿，否则会存在稳态横向偏差。

四、一个可手算的示例

设AUV位于 (x,y)=(10 m, 5 m)，航向 ψ=30°，航速 U=1.5 m/s。期望路径为沿x轴（正北）的直线，起点 (0,0)，即 α=0°、t_p=(1,0)、n_p=(0,1)。投影点为 (10,0)，于是 e = (0,1)·(0,5) = 5 m，表示AUV在路径右侧5 m处；ψ_p=0°，ψ_e=30°。代入近似方程得 ė≈1.5·sin30°=0.75 m/s，即横向误差正在以0.75 m/s的速度增大——这说明当前航向偏向路径右侧，若不修正，偏差会继续扩大。这一判断正是LOS制导律需要“向路径方向压舵”的直接依据。

把上述几何关系落到实现层面，路径跟踪模块通常按以下顺序工作：读取路径参数（直线用起点+方位角+长度，圆弧用圆心+半径+转向+圆心角，样条用航路点序列）；对当前AUV位置做最近点投影，得到 s、P_p、ψ_p、κ；计算 e 与 ψ_e；再交给制导律生成期望航向或转艏率。投影环节是数值上最需要小心的部分——直线和圆弧可解析求解，样条则需迭代，且要处理投影点越过路径端点时的“端点锁定”情形，否则误差会突然跳变。

最后强调两点工程约定。其一，e 的符号必须与 n_p 的定义绑定，控制器里若把法向量取成顺时针旋转90°，整个修正方向就会反号，表现为“越纠越偏”。其二，ψ_e 必须做角度归一化，否则在跨越 ±180° 时会出现数值跳变，导致转艏指令突变。这两条看似琐碎，却是路径跟踪调试中最常见的错误来源。