04.2 LOS 从投影到横向误差动力学：前视、横流与三维制动

零基础入口：为什么朝着终点走，还需要“投影”和“前视”

LOS 是 line of sight，即“视线方向”。这里看向的是路径上的虚拟目标，不要求真的有摄像机。直接朝最后终点游，可能切过弯道内侧；只朝最近路径点游，回到路径时又没有向前的意图。因此先找最近点，再沿路径向前取一点：前者负责回到路上，后者负责继续前进。

投影就是只保留沿路的分量。 两向量点积把同方向分量相乘相加；与单位方向点积，得到在该方向上的有符号长度。例如路径从 $a=(0,0)$ m 向 $b=(4,0)$ m，机器人在 $p=(1,2)$ m，向前单位方向为 $(1,0)$ 。沿路长度为 $(1,2)\cdot(1,0)=1$ m，离路侧向距离为 $2$ m。归一化的投影参数为 $\lambda=1/4=0.25$ ，含义是走完整段的四分之一，不是 $0.25$ m；投影点是 $(1,0)$ m。

取前视距离 $\Delta=1$ m，前视点就是 $(2,0)$ m。从机器人看过去的箭头为 $(1,-2)$ m：既向前，也向路径纠偏。若只取投影点，箭头变成 $(0,-2)$ m，便只横向回路，不再前进。前视越大，朝前越多、纠偏越温和；越小则纠偏越急，也更容易把测量噪声变成左右摆动。

atan2 不是一个神秘控制器。 $\operatorname{atan2}(y,x)$ 是从两个分量恢复箭头方向的函数，输出弧度，并保留象限；在 NED 水平面，本课把东分量放第一个参数、北分量放第二个参数。上例视线角是 $\operatorname{atan2}(-2,1)\approx-1.107$ rad，即向北偏西约 $63.4^\circ$ 。普通反正切只看比值，不能区分 $(1,1)$ 与 $(-1,-1)$ 两个相反方向；atan2 可以。两分量全为零则没有方向，必须单独处理。

本课首先研究运动学：给定速度，位置如何改变；例如向北 $0.2$ m/s 持续 $1$ s，理想位移为 $0.2$ m。动力学研究什么力、质量和阻力才能产生该速度；推进器饱和时速度未必立刻跟上。因此下文误差收敛先假设速度参考能被执行，再讨论制动、海流与跟踪误差，不能把制导公式直接当成推进器命令。

对应练习： 二维 LOS 在线练习。先把正横向误差画在纸上，确认命令向路径一侧，再写 atan2；本题不是完整有限线段导航器。

上一课给出一串路点，本课回答“此刻向哪里游、游多快”。先在一条无限直线上推导严格闭环，再处理有限线段、终点与三维速度约束。输入为路径段、当前位置与可信速度状态；输出是地理系速度参考，或欠驱动艇的航迹角参考。路径跟随不规定几点到达；轨迹跟踪才同时规定位置与时间。

符号、坐标与假设

符号	含义	维数 / 单位	坐标或约定
$a,b,p^n$	线段端点、当前位置	3×1 / m	NED
$\ell,\hat t,\lambda$	段长、单位切向、投影参数	m；3×1 无量纲；标量无量纲	路径几何
$\gamma,\chi,\psi,\beta$	路径角、地速航迹角、艏向、侧滑角	标量 / rad	北向为零、向东为正
$e,\Delta,U$	右侧横向误差、前视距离、地速大小	m；m；m/s	水平路径系
$V,k_I,I$	Lyapunov 函数、积分增益、误差积分	m²；1/s；m·s	教学积分 LOS
$v_c^n,v_r^n,v_g^n$	水流、水相对速度、地速	3×1 / m/s	NED
$a_b,a_{\perp},\tau,\kappa$	可用制动加速度、横向加速度、延迟、曲率	m/s²；m/s²；s；1/m	正的能力界
$n_\perp,p_{NE},e_\perp,I_3$	水平右法向、水平位置、三维法向误差、单位阵	2×1 无量纲；2×1 m；3×1 m；3×3	NE 平面或 NED
$U_r,d_\perp,p_L,p_\perp$	水相对前速、恒定横流分量、前视点、投影点	m/s；m/s；3×1 m；3×1 m	教学小角度流模型；几何点为 NED
$v_d^n,T_e,d$	地速参考、局部误差时间常数、可用停车距离	3×1 m/s；s；m	本课 d 是距离，不是上一课分辨率

初始推导假设路径是固定直线，投影在段内，实际地速能立即跟踪参考，且地速大小始终为正。它隔离了制导几何本身；随后把横流、速度内环、饱和与终点减速逐项放回去。这样能知道每条结论究竟来自哪一个假设，而不把一条 Lyapunov 不等式当作全系统稳定证明。

投影为什么是一个内积

\begin{aligned}\ell&=\|b-a\|,\quad\hat t=(b-a)/\ell,\\\lambda_0&=\frac{(p^n-a)^T(b-a)}{\ell^2},\quad\lambda=\operatorname{clip}(\lambda_0,0,1),\\p_\perp&=a+\lambda(b-a),\\p_L&=a+\min(1,\lambda+\Delta/\ell)(b-a),\\v_d^n&=U\frac{p_L-p^n}{\|p_L-p^n\|}.\end{aligned}

(4.5)

对线段上一点到机器人的距离平方求导，令导数为零，就得到未截断投影参数；若它落在区间外，最近点只能是端点，所以截断。前视距离除以段长后变成参数增量，这是量纲正确的原因。零长度线段应先跳过，机器人恰好位于前视点时也不能做除零归一化，而应进入到点或下一段逻辑。

\begin{aligned}\gamma&=\operatorname{atan2}(b_E-a_E,b_N-a_N),\\n_\perp&=\begin{bmatrix}-\sin\gamma\\\cos\gamma\end{bmatrix},\qquad e=n_\perp^T(p_{NE}-a_{NE}),\\\chi_d&=\gamma-\operatorname{atan2}(e,\Delta),\\\dot e&=n_\perp^T\dot p_{NE}=U\sin(\chi_d-\gamma)=-\frac{Ue}{\sqrt{\Delta^2+e^2}}.\end{aligned}

(4.6)

右侧法向与切向正交；当路径向北，正误差就是机器人偏东，修正必须带向西的速度，因此角度前面的负号可用物理图检验。投影点到前视点沿切向距离为前视长度，而机器人到投影点沿法向距离为负误差，直角三角形便给出 atan2。横向误差的导数只取速度的法向投影；代入正弦的三角形关系得到最后一式，每一步都没有使用推进器模型。

\begin{aligned}V&=\tfrac12e^2,\qquad\dot V=e\dot e=-\frac{Ue^2}{\sqrt{\Delta^2+e^2}}\le0,\\|e|&\ll\Delta\quad\Longrightarrow\quad\dot e\approx-\frac U\Delta e,\qquad T_e\approx\frac\Delta U.\end{aligned}

(4.7)

只要速度有正下界，非零误差就持续下降；接近路径时可用一阶指数系统近似，时间常数等于前视距离除以地速。大误差时横向速度接近地速上限，所以不能声称在任意初始误差下都有同一全局指数速率。增大前视会平滑方向也降低纠偏速度，减小前视则会放大位置噪声和内环滞后。Fossen 与 Pettersen 的原文进一步研究有界速度、前视和级联系统，TB04-S3；本课只证明上面明确写出的理想直线模型。

把投影和稳定性中跳过的代数补齐

为什么投影参数要除以段长平方？对未截断直线定义 $F(\lambda)=\|p^n-a-\lambda(b-a)\|^2$ 。展开内积后求导，得到

\begin{aligned} F'(\lambda)&=-2(b-a)^T(p^n-a)+2\lambda\|b-a\|^2,\\ F''(\lambda)&=2\|b-a\|^2>0. \end{aligned}

二阶导数为正说明驻点是唯一最小值；有限线段只是在此基础上增加区间约束。注意本课 $\lambda$ 是无量纲投影参数，不是视觉课的收敛增益。 $\ell$ 是整段长度， $\Delta$ 是投影之后要向前看的物理距离；它们只有在归一化参数增量 $\Delta/\ell$ 中相除。前视点被终点截断以后，实际沿切向的前视距离可能小于 $\Delta$ ，所以直线稳定性公式对应的是段内、尚未截断的情况。

对向北直线， $\gamma=0$ 、 $e=p_E-a_E$ ，速度的东分量必须为负才能纠正正误差。一般方向时，把北东速度投影到右法向，得到

\dot e=-U\sin\gamma\cos\chi+U\cos\gamma\sin\chi=U\sin(\chi-\gamma).

再用 $\chi=\gamma-\operatorname{atan2}(e,\Delta)$ ，三角形的斜边长为 $\sqrt{e^2+\Delta^2}$ ，正弦就是负对边除斜边。这样负反馈符号不是来自一行抽象定理，而是路径右侧必须往左游的几何事实。选择 $V=e^2/2$ 是因为它非负且只有零误差处为零，导数 $e\dot e$ 又恰好暴露纠偏方向；它不是实际机械能，单位为 m²。

若只知 $\dot V\le0$ ，一般不能直接宣布所有状态收敛；本课可进一步使用标量方程：非零误差始终向零运动，且 $U\ge U_{\min}>0$ 排除了“速度越来越小、走不到路径”的情形。在限定误差范围 $|e|\le E$ 、固定前视下，还可用

\dot V\le-\frac{2U_{\min}}{\sqrt{\Delta^2+E^2}}V

得到该范围内的指数界。 $E$ 是初始允许误差上界，单位 m；它越大，这个统一速率界越保守。这就是为什么不能把局部时间常数 $\Delta/U$ 无条件推广到任意远处。

横流为什么使“角度直接当艏向”失效

\begin{aligned}v_g^n&=v_r^n+v_c^n,\qquad\chi=\psi+\beta,\\\psi_d&=\chi_d-\hat\beta,\\v_{r,d}^n&=v_{g,d}^n-\hat v_c^n,\qquad v_{r,d}^b=(R_b^n)^T v_{r,d}^n.\end{aligned}

(4.8)

欠驱动艇不能独立施加水平横向速度，通常用前进与转艏改变运动方向；有横流时，艏向与地速方向不一致，必须估计侧滑或水流，或让积分制导自动建立迎流角。可独立侧移的 ROV 则可让艏向继续面对目标，同时执行 NED 横向速度，不能强迫它套用“速度方向等于船头方向”的运动学。若下层已经以底跟踪地速闭环，给它的仍应是地速参考；减去水流只适用于水相对模型或前馈通道，否则会重复补偿。

\begin{aligned}\psi_d&=\gamma-\operatorname{atan2}(e+k_I I,\Delta),\qquad\dot I=e,\\\dot e&\approx-\frac{U_r}{\Delta}e-\frac{U_r k_I}{\Delta}I+d_\perp,\\\ddot e&\approx-\frac{U_r}{\Delta}\dot e-\frac{U_r k_I}{\Delta}e,\quad \dot d_\perp=0.\end{aligned}

(4.9)

这里单独构造一个小角度、恒定水相对前速的教学积分 LOS：右偏持续存在，积分逐渐积累，产生额外左转，即使误差归零也保留迎流角。局部二阶特征多项式的两个系数为正，所以正增益给出局部稳定；平衡时积分抵消恒定横流。它不是 Fossen–Lekkas 自适应算法的完整复写：该论文专门区分相对速度与绝对速度，并构造流观测器或自适应律，TB04-S4。大误差、饱和和强流下，简单积分会卷绕；冻结或回算积分、限制参考并重新检验可行性，比分母里随意加一个小数更重要。

手算：水相对速度 0.3 m/s，向东横流 0.06 m/s，想沿正北航行。所需向西速度为 0.06 m/s，因此迎流艏向约为负 11.54°，而地速北分量约 0.2939 m/s。若直接令艏向正北，机器人必然持续向东偏。流速若超过可提供的横向补偿能力，则不存在保持该路径的稳态，积分再大也不能创造推力。

积分究竟记住了什么：平衡点、特征根与离散更新

教学 PI-LOS 中， $I$ 不是“额外角度”，而是横向误差对时间的积分，单位 m·s； $k_I$ 的单位为 $1/\mathrm{s}$ ，所以 $e+k_I I$ 仍是米，才能与前视距离一起进入 atan2。在小角度下， $\sin\alpha\approx\alpha$ 且 $\operatorname{atan2}(z,\Delta)\approx z/\Delta$ 。把相对水速在法向的分量与恒定横流 $d_\perp$ 相加，就得到上面的线性化误差方程，而非凭空给 LOS 加一个 PID。

令误差和积分导数都为零，平衡积分是

I^*=\frac{d_\perp\Delta}{U_r k_I}.

它所保存的是抵消横流所需的持久转向偏置，而不是“机器人以前走偏了多少米”。用 $\tilde I=I-I^*$ 消去常值扰动，写成状态方程：

\begin{bmatrix}\dot e\\\dot{\tilde I}\end{bmatrix} =\begin{bmatrix}-U_r/\Delta&-U_rk_I/\Delta\\1&0\end{bmatrix} \begin{bmatrix}e\\\tilde I\end{bmatrix},\qquad s^2+\frac{U_r}{\Delta}s+\frac{U_rk_I}{\Delta}=0.

这里 $s$ 为特征方程变量，单位 $1/\mathrm{s}$ ，不是前面线段插值参数。二阶实系数多项式两个系数都为正，其根具有负实部：实根情形，根和为负、根积为正，故都负；共轭根情形，实部等于负的一次项系数的一半。 $U_r=0.3$ m/s、 $\Delta=1$ m、 $k_I=0.15/\mathrm{s}$ 时，根为 $-0.15\pm0.15j$ 每秒；横流 $0.06$ m/s 对应 $I^*=1.3333$ m·s。它可能带衰减摆动，但能消除该小角度恒流模型的稳态误差。

C++ 示例按当前误差先算导数，再同时前进一步。用前向欧拉时，对每个连续根 $s_i$ ，离散根为 $1+\Delta t\,s_i$ ；必须落在单位圆内，才不会把连续稳定系统离散成发散系统。示例 $\Delta t=0.01$ s 远小于误差时间尺度。实际欠驱动艇的艏向内环滞后、积分限幅和水流变化会增加状态；若命令已经饱和却继续积分，保存的就不再是可实现的迎流偏置，因此需要冻结、回算或限幅策略。

三维速度、制动与曲率

\begin{aligned}e_\perp&=(I_3-\hat t\hat t^T)(p^n-a),\\v_d^n&=U\frac{\Delta\hat t-e_\perp}{\sqrt{\Delta^2+\|e_\perp\|^2}},\\\frac{d}{dt}\frac{\|e_\perp\|^2}{2}&=-\frac{U\|e_\perp\|^2}{\sqrt{\Delta^2+\|e_\perp\|^2}}.\end{aligned}

(4.10)

三维直线可直接用投影矩阵除去切向分量，余下两个方向一起纠偏，纯竖直线也不退化。最后一行来自固定切向的投影矩阵幂等性；它需要实际三维速度跟踪。若只限 Z 分量而不同比例缩放其余分量，方向被改变，上面的等式不再原样成立。若希望保留方向，可先求各轴允许缩放比例及总速比例，取最小值同乘整个向量；接近终点时另用位置误差产生趋零参考。

\begin{aligned}d_{\rm stop}&=U\tau+\frac{U^2}{2a_b},\\U_{\rm stop}(d)&=-a_b\tau+\sqrt{(a_b\tau)^2+2a_b d},\\a_{\perp,\rm req}&=U^2|\kappa|,\qquad U_{\rm curve}\le\sqrt{a_{\perp}/|\kappa|}.\end{aligned}

(4.11)

先在延迟期间匀速运动，再以恒定可用减速度制动，把两个距离相加得到停车距离；解对应二次方程得到距离允许的速度上限。这里的距离应为沿候选轨迹剩余的可用净距，不是任意障碍的直线距离。曲率公式来自切向单位矢量随弧长的变化；折线路点处曲率不有限，需要停下转向或使用碰撞复检后的圆滑连接。三维中制动能力随方向变化，应使用当前方向能保证的最小可用能力。

例子：线段从 (0,0,0) 到 (4,0,0)，当前位置 (1,0.6,0)，前视 1 m、速度 0.3 m/s。投影为 (1,0,0)，前视点为 (2,0,0)，速度约 (0.25725,−0.15435,0) m/s，误差导数为负，Lyapunov 导数约 −0.09261 m²/s。若可用制动为 0.15 m/s²、总延迟 0.2 s，停车距离为 0.36 m；若只剩 0.2 m，则允许速度约 0.21678 m/s，不能继续保持 0.3。

先手写，再展开参考实现

将本课公式写成一个 C++17 单文件程序。先列输入单位与退化分支，再写核心循环；不要先看答案再照抄。参考程序只依赖标准库，可用 g++ -std=c++17 -O2 lesson.cpp -o lesson && ./lesson 离线运行，不访问机器人。断言是对上文手算结果的检查，不是用程序输出代替推导。

参考解答（展开查看）

// 教学实现；纯标准库、离线，不发送命令。
#include <array>
#include <cmath>
#include <algorithm>
#include <iostream>
#include <cassert>
using V=std::array<double,3>;
V sub(V a,V b){for(int i=0;i<3;++i)a[i]-=b[i];return a;}
V add(V a,V b){for(int i=0;i<3;++i)a[i]+=b[i];return a;}
V mul(V a,double s){for(double &x:a)x*=s;return a;}
double dot(V a,V b){double s=0;for(int i=0;i<3;++i)s+=a[i]*b[i];return s;}
double norm(V a){return std::sqrt(dot(a,a));}
V los(V p,V a,V b,double look=1,double speed=.3){
    V d=sub(b,a);double length=norm(d);
    if(length<1e-9)return {}; // 调用者负责跳过重复航点
    double t=std::clamp(dot(sub(p,a),d)/(length*length),0.,1.);
    V target=add(a,mul(d,std::min(1.,t+look/length)));
    V delta=sub(target,p);double r=norm(delta);
    if(r<1e-9)return {};
    V out=mul(delta,speed/r);V caps{.35,.35,.1};
    double scale=std::min(1.,.4/std::max(norm(out),1e-9));
    for(int i=0;i<3;++i)if(std::abs(out[i])>1e-9)
        scale=std::min(scale,caps[i]/std::abs(out[i]));
    return mul(out,scale); // 同比缩放，保留方向
}
double stopSpeed(double d,double a,double latency){
    assert(d>=0 && a>0 && latency>=0);
    return std::sqrt(a*a*latency*latency+2*a*d)-a*latency;
}
int main(){
    V v=los({1,.6,0},{0,0,0},{4,0,0});
    assert(v[1]<0);assert(std::abs(norm(v)-.3)<1e-12);
    assert(std::abs(v[1]+.154348726628)<1e-10);
    auto vertical=los({0,0,0},{0,0,0},{0,0,2});
    assert(std::abs(vertical[2]-.1)<1e-12);
    std::cout.precision(12);
    std::cout<<v[0]<<' '<<v[1]<<' '<<v[2]<<'\n';
    std::cout<<stopSpeed(.2,.15,.2)<<" m/s\n";
    // 恒流、小角度教学积分 LOS；积分单位 m*s。
    double e=.6,I=0,dt=.01,U=.3,Delta=1,ki=.15,crossCurrent=.06;
    for(int k=0;k<20000;++k){
        double edot=-U/Delta*e-U*ki/Delta*I+crossCurrent;
        I+=dt*e;e+=dt*edot;
    }
    assert(std::abs(e)<1e-8);
    std::cout<<e<<' '<<I<<" (expected I="<<crossCurrent*Delta/(U*ki)<<")\n";
}

项目函数与手写作业

沿 nav_los.py 的 LOSNavigator.update 阅读投影、接受半径、临近目标降速、分量限幅和总速约束，再读 mavlink_velocity.py 的 MavlinkVelocitySender。项目示例为 10 Hz、默认速度 0.3 m/s、前视 1 m，水平分量上限 0.35 m/s、垂向 0.1 m/s、总速 0.4 m/s；纯深度航点使用更紧接受半径。将实现的逐轴限幅与上面教学版同比缩放作比较，观察哪一种更能保留制导方向。

用 $0.01$ s 步长从横向误差 $0.6$ m 开始，比较前视 $0.5,1,2$ m；再加入有限转艏速度。
接近终点时速度趋零，原直线证明中哪一个条件失效？该增加哪一层终点逻辑？
对纯竖直路径调用自己的 C++ 制导函数，检查水平投影退化是否会导致除零。

参考解答（展开查看）

练习：写欧拉积分仿真，用 0.01 s 步长从横向误差 0.6 m 开始，比较前视 0.5、1、2 m。答案要点：误差都向零走，局部时间常数分别约 1.67、3.33、6.67 s；加入有限角速度后，小前视可能反而产生摆动。第二题把终点速度逐渐降为零：为什么原有“持续下降”不能直接证明最终到达？因为正速度下界消失，需增加终点位置闭环或有限距离推进逻辑。第三题对竖直段调用代码：结果应仅沿深度及垂向于该线的误差方向修正，而不是计算未定义的水平航向。

一次制导更新应按什么顺序运行

先读同一时刻的位置与健康状态，选当前有效路径段并跳过重复路点；求投影和前视点；根据横向误差求方向；再由终点剩余距离、曲率和可用制动选速度；做与坐标一致的限幅；最后按消息契约发布 NED 速度。到点逻辑负责推进段索引，终点逻辑负责把“继续沿线游”改成“收敛到最后位置”。如果先限幅后再任意旋转，轴向上限可能不再满足；如果先切换航点再评估距离，纯深度段可能尚未到达就被跳过。阅读项目的 LOSNavigator.update 时，按这个顺序标注每一小段代码负责哪一个物理决定。

TB04-S3 · Fossen & Pettersen：On Uniform Semiglobal Exponential Stability of Proportional LOS Guidance Laws

2014 作者预印本。依次读 cross-track error、lookahead-based guidance、速度和前视界，再看级联讨论。

TB04-S4 · Fossen & Lekkas：Direct and indirect adaptive integral LOS path-following controllers

2015 在线发表、期刊卷期为 2017。优先辨认相对水速与地速，再读流观测器和直接自适应方法；不要将教学 PI 近似当作其完整控制律。