4. LOS制导基本原理与几何推导

视线导引（Line-of-Sight, LOS）是AUV路径跟踪中最经典、工程上最常用的制导方法。它的核心思想非常朴素：在期望路径上选取一个“瞄准点”，让航行器始终朝这个点飞行，随着航行器前进，瞄准点沿路径向前滑动，从而形成对整条路径的跟踪。理解LOS，关键不在于记住公式，而在于把“路径—瞄准点—航行器”三者的几何关系看清楚，并理解前视距离如何调节这种关系。

一、从“看哪里”到“怎么转”：LOS的几何图像

先约定坐标系与符号，全文保持一致：采用北东（NED）水平面坐标系，路径以参数化曲线给出；航行器位置为 P(x, y)，航向角 ψ 定义为相对正北方向顺时针为正；期望航向角记为 ψ_d，航向误差 ψ_e = ψ_d − ψ。设期望路径上距离航行器最近的点为 P_k(x_k, y_k)，称为投影点或参考点；在路径前进方向上，从 P_k 沿路径向前取一段弧长 Δ，得到瞄准点 P_{los}(x_{los}, y_{los})。这段弧长 Δ 就是前视距离。

于是LOS的几何问题化为：已知航行器当前位置与瞄准点位置，求从航行器指向瞄准点的方向角，这个角就是期望航向角。设两点间横向偏差为 e（航行器到路径的有符号距离，左正右负），纵向沿路径方向的投影差为 Δ，则从航行器看向瞄准点的方向相对路径切线的偏角为：

ψ_los = arctan( e / Δ )

再叠加路径在投影点处的切线方向角 ψ_p，即得期望航向角：

ψ_d = ψ_p + arctan( e / Δ )

这就是LOS制导律最基本的几何形式。它的物理含义是：横向偏差 e 越大，瞄准方向偏离路径切线越多，制导指令越“用力”地把航行器拉回路径；前视距离 Δ 越大，同样的偏差对应的偏角越小，制导越“温和”。

二、前视距离的作用：稳定性与收敛性的折中

前视距离 Δ 是LOS制导中最重要的可调参数，它直接决定了闭环系统的“性格”。从公式 arctan(e/Δ) 可以看出，Δ 相当于把横向偏差“归一化”的尺度：

• Δ 偏小：瞄准点靠近投影点，航行器几乎盯着路径本身飞，偏差被迅速放大成较大的转向指令。好处是收敛快、路径贴合紧；代价是容易产生过冲和振荡，尤其在航向响应有延迟、存在侧向流或舵机饱和时，可能出现“来回摆”的现象。
• Δ 偏大：瞄准点远在前方，制导指令平缓，航向变化柔和，系统稳定、抗扰性好；但横向偏差收敛慢，遇到急弯时容易“切内角”，跟踪误差在弯道处明显增大。

因此工程上常采用“随速度或随曲率自适应”的前视距离：直线段用较大 Δ 保证平稳，弯道段减小 Δ 提高贴合度。一个常用的经验形式是让 Δ 与航速 U 成正比，例如 Δ = k·U，k 取 1～3 秒量级，使瞄准点对应一个大致固定的“预瞄时间”。

三、一个可手算的示例

假设某AUV在北东坐标系下位于 P(0, 0)，期望路径为沿正北方向的直线，即 ψ_p = 0。航行器实际偏在路径右侧 4 m，按“左正右负”约定，横向偏差 e = −4 m。取前视距离 Δ = 20 m，则：

ψ_los = arctan(−4 / 20) = arctan(−0.2) ≈ −11.3°
ψ_d   = 0 + (−11.3°) = −11.3°

负号表示期望航向偏向正北以西，即向左修正，把航行器拉回路径，方向正确。若把 Δ 改为 8 m，则 ψ_los = arctan(−0.5) ≈ −26.6°，修正力度明显加大。这个对比直观说明：同样的 4 m 偏差，前视距离从 20 m 缩到 8 m，制导指令从约 11° 增大到约 27°，收敛更快但更易振荡。

四、从几何到制导律：还需要补上的两件事

上面的推导给出的是“期望航向角”，它属于运动学层面的制导指令。要真正闭环，还需要：其一，把 ψ_e 送入航向控制器（如PID或反步法），输出舵角或推力差；其二，处理投影点的在线计算——对折线路径，投影点是当前航段上的最近点，需判断是否越过航段端点并切换到下一段；对曲线路径，则用数值方法求最近点。工程实现中，投影点计算错误是路径跟踪“跑偏”的常见根源，尤其在航段切换处要加迟滞判断，避免在端点附近反复切换。