我最早接触UUV三维路径跟踪，是在帮人调一款水下潜航仿真模型的时候。当时拿到手的代码是传统轨迹跟踪思路，给定一条带时间戳的位置序列，让控制器去逐点追，结果螺旋路径稍微拐得急一点，整个系统就开始来回震荡。折腾了一个多星期，最后换成了“LOS制导 + PID控制”的结构，问题才真正解开。
这个标题是不少人毕业设计或项目课题里的常客：基于LOS制导+PID控制的无人潜艇UUV三维路径跟踪。它的核心任务是让UUV沿着一条预设的三维空间路径（比如螺旋下潜、折线巡航）自主航行，整体精度和稳定性都要可控。本文我就以这套方案的完整实现过程为主线，把运动学建模、LOS制导律的原理、三通道PID控制器设计、MATLAB仿真代码拆解以及调参阶段踩过的坑一次讲清楚，适合刚开始接触水下机器人控制或想从二维水面船控制过渡到三维水下控制的朋友参考。
1. 先搞清楚：三维路径跟踪和轨迹跟踪根本不是一回事
1.1 一个时间维度的差异，导致控制思路完全不同
很多初学者拿到这个题目，第一反应是“给一条路径，让UUV跟着走就行了”，然后把路径离散成一系列带时间的坐标点，当成轨迹跟踪来做。轨迹跟踪强调的是“某个时刻必须到达某个位置”，速度波动会直接转化为位置误差，控制器必须同时处理空间和时间的双重偏差。
路径跟踪则完全不同，它只要求UUV在空间上收敛到给定路径上，路径本身不绑定严格的时间节拍。水下环境水流扰动大，UUV速度经常浮动，要求每个时刻准点到达某个点既不现实，也没有工程必要。路径跟踪放弃时间约束后，控制器只需要关注几何位置偏差，问题一下子简化了很多。LOS制导正是为路径跟踪这种需求设计的：它天然不关心“时间点”，只关心“你离那条线还有多远、偏在哪一侧”。
1.2 运动学模型与坐标约定
做仿真前，先把模型坐标系定下来，这一步最枯燥但最致命。本文采用工程上最常见的北东地（NED）坐标系约定：x轴指北，y轴指东，z轴向下为正。UUV的位置向量记为 (P=[x,y,z]^T)，姿态用横摇角φ、俯仰角θ、偏航角ψ描述。水下匀速巡航场景下，横摇影响可以忽略，运动学模型简化为：
[ \dot{x} = u\cos\psi\cos\theta ] [ \dot{y} = u\sin\psi\cos\theta ] [ \dot{z} = u\sin\theta ]
[ \dot{\psi} = r,\quad \dot{\theta} = q ]
其中u是UUV的前向速度，r是偏航角速度，q是俯仰角速度。这个模型把UUV抽象成一个带姿态的质点，推进器负责u，舵面负责r和q。整个控制系统的结构随后就定了：外环用LOS制导根据位置误差计算期望的ψ_d和θ_d，内环用PID控制实际航向和俯仰去跟踪这个期望角，速度环维持巡航速度u。
这个级联结构是整个仿真的骨架，后续所有代码里的变量定义、控制器职责划分都从这里来。很多人后期调参调发散，往往就是忘了这个层级关系，直接用PID去怼位置误差，等于把制导和控制的职责混在了一起。
2. LOS制导在三维里到底是什么：一个追灯塔的几何游戏
2.1 二维LOS的几何直觉
LOS全称Line-of-Sight，中文学名视线法。二维情况下可以这样理解：在参考路径上找一个“前瞻点”，让船舶始终朝这个点航行。这个点不是最近点，而是船前方一定距离处的路径点，就像夜航时盯住远处塔尖上的灯，而不是盯着脚下。
设船到路径的横向偏差为e_y，前瞻距离为Δ，路径在该点的切线方向角为ψ_p，则LOS期望航向角为：
[ \psi_d = \psi_p + \arctan(-e_y / \Delta) ]
e_y为正（船在路径右侧）时，修正角为负，指令船向左回靠；e_y越大，回靠越急。Δ决定“看多远”：Δ大，修正平缓、轨迹平滑；Δ小，修正剧烈、容易振荡。经典经验是Δ取UUV长度的2到3倍。
这个公式的巧妙之处在于，它通过一个可调参数Δ，把路径切线方向和一个基于位置偏差的比例修正项组合起来。本质上和走山路抬头看路是同一个道理——看远一点，走得稳一点。
2.2 三维推广：把视线角拆到两个平面里
三维空间里原理不变，只是要把视线同时投影到水平面和垂直面处理。水平面看横向偏差e_y和水平前视距离Δ_h，得到偏航指令；垂直面看深度偏差e_h和垂直前视距离Δ_v，得到俯仰指令：
[ \psi_d = \psi_p + \arctan(-e_y / \Delta_h) ]
[ \theta_d = \theta_p + \arctan(-e_h / \Delta_v) ]
其中ψ_p、θ_p是参考点处路径的切线方位角和倾角。这是一套“水平垂直解耦”的处理方式，够简单，实际工程中也最常用。
稍微精细一点的方案是垂直面采用耦合形式：
[ \theta_d = \theta_p + \arctan\left(-e_h / \sqrt{\Delta_h^2 + e_y^2}\right) ]
意思是横向偏差越大，垂直方向的“视野”越远，俯仰修正越平缓，避免出现“弯都没转过来就开始猛爬升”的别扭轨迹。两者我都试过，螺旋下潜这类路径用解耦版就足够，只有初始误差很大时耦合版明显更稳。
2.3 谁是你的“灯塔”：参考点选取与路径参数更新
LOS里的参考点并不是随便取的，它必须沿着给定路径平滑移动，且移动速度要与UUV在切线方向的投影速度匹配。路径通常用参数方程 (P_r(s)=[x_r(s), y_r(s), z_r(s)]^T) 描述，s是路径弧长参数。参考点更新率的基本思路是：让参考点前进的速率等于UUV速度沿路径切线方向的分量，再除以路径自身的拉伸系数：
[ \dot{s} = \frac{u\cos(\psi-\psi_p)\cos(\theta-\theta_p)}{\sqrt{x_r'^2 + y_r'^2 + z_r'^2}} ]
这里x_r'、y_r'、z_r'是路径对s的导数。分母其实就是路径切向量模长，做归一化用的。
这个更新律是整个LOS制导真正有技术含量的地方。如果ṡ算得偏大，参考点会“抛下”UUV往前跑，导致跟踪误差永远消不掉；如果偏小，参考点又会被UUV追着跑，出现滞后。很多仿真里那种始终无法消除的稳态误差，八成出在这一步。
3. PID控制器设计：三个通道，三种脾气
3.1 航向通道：PD打好基础，I只用来收尾
偏航控制器是最先要调的。它负责让实际ψ跟踪LOS给定的ψ_d。但请注意，PID控制器输出的是偏航角速度指令r，而不是直接输出舵角，舵角映射是在动力学层面完成的。运动学仿真里这样做信息匹配。
航向通道的调参顺序很固定：先去掉积分项，只调比例和微分，让阶跃响应快且不超调；稳定后如果发现有固定静差，再加一个很小的Ki。水下无人艇的航向静差主要来自流和推进器不对称，一般不大的Ki就够了：
% 航向PID，输出偏航角速度 persistent integral_psi prev_err_psi dt = 0.05; Kp_psi = 1.2; Ki_psi = 0.05; Kd_psi = 0.8; err = wrapToPi(psi_d - psi); % 角度差注意归一化 integral_psi = integral_psi + err * dt; integral_psi = max(min(integral_psi, 0.5), -0.5); % 积分限幅 deriv = (err - prev_err_psi) / dt; r_cmd = Kp_psi * err + Ki_psi * integral_psi + Kd_psi * deriv; prev_err_psi = err;角度偏差归一化到[-π, π]非常重要，否则无人机航向从-179度转到+179度时，误差会被算成358度而不是2度。
3.2 深度通道：限幅和积分饱和是第一优先级
垂直面控制比水平面麻烦一些，因为UUV在水下受浮力和重力配平的影响，系统本身存在常量偏置。这意味着深度通道必须有积分项，不然会一直存在固定静差；但同时积分饱和现象在深度通道出现得也最频繁。
工程上的做法是三层保险：第一，俯仰角指令限幅，典型范围±30°，超过这个值UUV失速风险很大；第二，积分项单独限幅，只允许输出总控制量的30%左右；第三，只有当深度误差低于某个阈值时才允许积分累计，防止启动瞬间误差巨大导致积分快速打满。
% 俯仰PID，输出俯仰角速度 err_theta = wrapToPi(theta_d - theta); if abs(err_theta) < 5*pi/180 integral_theta = integral_theta + err_theta * dt; end integral_theta = max(min(integral_theta, 0.3), -0.3); q_cmd = Kp_theta * err_theta + Ki_theta * integral_theta + Kd_theta * deriv_theta;这里有个容易被忽略的点：俯仰角误差同样需要wrapToPi处理。很多仿真代码里转到-250度时误差跳变，系统直接发散，就是忘了做角度归一化。
3.3 速度通道与外环的配合
速度环一般最简单，用PI控制维持u_d即可。速度环的目的是让LOS参数更新律里那个u可以被当成一个稳定变量，而不是让整个系统陷入位置-速度-姿态的多重耦合。
但速度环与制导不是完全无关的：前视距离Δ最好随速度缩放。回头想想，船速快的时候，同样时间内前进的路径更长，需要看得更远才能保持相同的响应特性。可以设Δ = k·u，k取2到3秒，这样速度从1m/s变到3m/s时，制导律的等效响应频率不会漂移。
4. MATLAB实现拆解：从路径到闭环只有五个模块
4.1 路径定义与参考点投影
先写一条三维螺旋下潜路径，这是验证三维路径跟踪最典型的场景。用弧长参数s表示：
function P = spiralPath(s) R = 50; % 水平半径 k = 0.05; % 弧长与角度换算系数 depth_rate = 0.3; % 每米弧长下潜0.3米 x = R * cos(k*s); y = R * sin(k*s); z = depth_rate * s; % NED坐标系，z向下为正 P = [x; y; z]; end切线方向可以用解析导数，也可以用数值差分。解析导数更稳，避免在参数步长过大时把切线方向算歪：
function T = spiralTangent(s) R = 50; k = 0.05; depth_rate = 0.3; dx = -R*k*sin(k*s); dy = R*k*cos(k*s); dz = depth_rate; T = [dx; dy; dz]; T = T / norm(T); end4.2 参考点更新与误差坐标变换
参考点更新采用第2.3节的公式，代码里注意对s_dot设置上限，防止数值异常：
s_dot = u * cos(psi - psi_p) * cos(theta - theta_p) / norm(tangent); s_dot = min(s_dot, 5); % 限速 s = s + s_dot * dt;误差变换这一步是三维相对二维最容易写错的地方。不要直接在全局坐标系里用z-z_r算深度误差，要把位置差投影到路径坐标系的三个基向量上。路径坐标系基向量由切线t、水平法向n、垂直法向b组成：
t = tangent; t_h = [t(1); t(2); 0]; t_h = t_h / norm(t_h); n = [-t_h(2); t_h(1); 0]; % 水平法向，指向路径左侧 b = cross(t, n); % 垂直方向 err_vec = P - P_r; e_t = err_vec' * t; % 切向误差，用来辅助理解，不参与横向控制 e_y = err_vec' * n; % 横向误差 e_h = err_vec' * b; % 垂直误差注意这里的b是三维向量，在螺旋下潜路径上它不完全指向铅垂方向，而是跟随路径倾斜。这个细节让垂直误差计算更准确，并且天然考虑了水平面偏航和垂直面俯仰的耦合。
4.3 LOS制导律与PID控制量计算
拿到e_y、e_h后，按解耦LOS计算期望角：
delta_h = 2 * L; % L为UUV长度 psi_d = psi_p + atan2(-e_y, delta_h); theta_d = theta_p + atan2(-e_h, delta_h); % 限幅 theta_d = max(min(theta_d, 30*pi/180), -30*pi/180);注意这里用atan2而不是arctan，Python和MATLAB里都一样推荐，能正确处理象限问题。然后PID输出r_cmd和q_cmd，如第3章代码。
4.4 运动学更新与主循环
运动学更新直接用第1.2节的模型，用欧拉法或四阶龙格库塔积分。欧拉法在dt=0.01s下精度尚可，dt超过0.05s建议换RK4。把整个闭环串起来，主循环骨架如下：
for k = 1:N % 1. 路径参数更新 s_dot = compute_s_dot(...); s = s + s_dot * dt; % 2. 获取参考点和基向量 [P_r, t, n, b] = getPathFrame(s); % 3. 计算路径坐标系误差 [e_t, e_y, e_h] = computeErrors(P, P_r, t, n, b); % 4. 计算LOS期望角 [psi_d, theta_d] = losGuidance(e_y, e_h, psi_p, theta_p, delta_h); % 5. 内环PID，得到角速度指令 r_cmd = pidYaw(psi_d, psi); q_cmd = pidPitch(theta_d, theta); % 6. 运动学更新 [x, y, z, psi, theta] = kinematicUpdate(..., dt); % 7. 记录数据 log(k,:) = [x, y, z, psi, theta, e_y, e_h]; end可视化方面，用plot3画出参考路径和实际轨迹，再把e_y、e_h随时间的变化画成两条曲线，观察收敛过程。只画三维轨迹而不画误差曲线是新手常犯的错，三维曲线只能看整体，网格状误差曲线才能看出收敛特性和振荡频率。
5. 仿真里最常翻车的五个问题
5.1 参考点更新过快：蛇形振荡的根源
首次闭环仿真最常见的现象：UUV绕路径左右摆动，幅度越来越大，或者永远没法收拢。很多人怀疑PID参数，其实问题往往在参考点更新率ṡ过大，LOS的“灯塔”一直在往前跑，船追着看不见的目标走，自然无法收敛。
解决办法有两个。一是给ṡ加限幅，二是用最近点投影替代单纯的积分更新。我实际更推荐后者：每个控制周期先沿路径搜索离当前位置最近的参考点（s的局部搜索），再在这个基础上叠加一个小前瞻offset。这样做鲁棒性高很多，只是计算量略增。
5.2 俯仰角接近±90度时模型退化
运动学模型里z轴更新用的是sinθ，当θ接近±90度时，水平面运动方程cosθ趋近零，整个水平通道就“锁死”了。而LOS在初始误差很大的情况下可能给出极端的俯仰角指令，瞬间把系统推进奇异区域。
工程处理是双重的：一是限幅，把θ_d限制在±30度，同时限制q_cmd；二是在大误差初始场景下，先用一段过渡路径或较大前视距离让UUV平缓切入，而不是直接命令它朝目标猛转。这个属于“你的控制律再好，也要给执行机构留活路”的原则。
5.3 深度通道积分饱和：静差越消越大
水下UUV的浮力配平会造成持续偏差，这需要积分项来抵消，但积分项也是最容易失控的环节。如果仿真开始时就有一个很大的初始深度误差，误差累加会让积分项瞬间饱和，等到实际误差快收敛时积分还在反向施加控制，系统就会严重超调，俗称“追尾”效应。
我的经验是：积分限幅设成总输出的20%到30%，并且只有误差进入死区才开始积分。不需要让积分项帮助完成大误差收敛，那本来就是比例和微分的工作。
5.4 量纲、符号和坐标约定混乱
这个坑看着低级，杀伤力极大。u是米每秒，r是弧度每秒，控制器的比例系数Kp的单位是“每秒每弧度”，如果写成“每弧度每秒”，数值上差整整一个数量级。所有参数看着合理，但系统就是慢半拍或者抖成筛子。
另一个高发问题是坐标方向符号。NED坐标系z向下为正，那么下潜方向z增大；如果路径定义用了“高度向上为正”的约定，LOS公式里的垂直误差就得反号。整个项目里我通常保证路径、运动学、误差计算三处用的同一个坐标约定，并在每个函数开头写一行注释标明。
5.5 初值误差过大：动态前视距离救场
固定前视距离Δ=2L在大多数场景下没问题，但如果初始横向误差达到了10L以上，LOS给出的期望偏航角会在很长一段时间里处于饱和状态，UUV几乎是横着漂向路径，切入角很丑且容易超调。
动态LOS是一种改动极小、收益极大的方案：让Δ随误差模值变化：
[ \Delta = \Delta_{min} + \beta |e_y| ]
误差大时前视距离拉长，UUV沿一个大圆弧平滑接近路径；误差收敛后恢复小Δ，保证跟踪精度。这个三行代码的改动，在很多初始偏差很大的场景里直接决定了系统能不能收敛。我在给A同学调参数时，就靠这个把发散的系统救回来过一次。
最后分享一个个人调试习惯，供参考：每次跑仿真都先把参考点轨迹和路径画在一张图里，先看“灯塔”的行为，再介入控制器参数。LOS制导里的参考点如果走得不顺，后面PID怎么调都是白费。先把几何层面调顺，再调动力学层面，顺序不要反。
