1. 项目背景与核心需求
水下无人潜航器（UUV）的自主导航一直是海洋工程领域的难点。传统航路点跟踪方法在复杂海流环境下容易出现轨迹偏移和振荡现象，特别是在三维空间中的深度控制尤为关键。这个项目通过结合视线制导（LOS）算法与PID控制器，实现了UUV在三维空间中的稳定路径跟踪。
我在实际海洋测试中发现，纯PID控制在遇到强侧向海流时，UUV容易产生"之"字形路径（俗称蛇形机动），不仅增加能耗，还会影响传感器数据采集质量。而LOS制导能提供更平滑的航向角指令，配合PID的动态调节，可以显著提升跟踪精度。
2. 系统架构设计
2.1 整体控制流程
系统采用分层控制结构：
code复制[路径规划] → [LOS制导层] → [PID控制层] → [推进系统]
↑ ↑
[位置反馈] [姿态/速度反馈]
2.2 LOS制导原理
LOS的核心是计算视线角（Look-Ahead Angle）。在三维空间中需要同时计算水平面和垂直面的视线角：
matlab复制% 水平面视线角计算
psi_d = atan2(y_la - y, x_la - x);
% 垂直面视线角计算
theta_d = atan2(z_la - z, sqrt((x_la-x)^2 + (y_la-y)^2));
其中(x_la,y_la,z_la)是前视点坐标，Δ是前视距离（通常取2-3倍艇长）
关键经验：前视距离Δ需要根据UUV速度动态调整。我们通过实测发现最佳关系式为 Δ = max(2L, 0.3v)，其中L为艇长，v为速度(m/s)
2.3 PID控制器设计
采用双闭环PID结构：
- 外环：位置控制（输出期望姿态角）
- 内环：姿态控制（输出推进器指令）
matlab复制% 外环PID示例
err_pos = [x_d-x; y_d-y; z_d-z];
att_d = Kp_pos.*err_pos + Ki_pos.*cumsum(err_pos)*dt;
3. 三维路径跟踪实现
3.1 坐标系转换
需要处理三个坐标系间的转换：
- 惯性坐标系（NED）
- 体坐标系（Body）
- 路径坐标系（Path）
转换矩阵特别要注意欧拉角顺序（通常采用Z-Y-X顺序）：
matlab复制R = [cos(psi)*cos(theta), -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi), sin(psi)*sin(phi)+cos(psi)*sin(theta)*cos(phi);
sin(psi)*cos(theta), cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi), -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi);
-sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi)];
3.2 自适应前视距离
固定前视距离在转弯处会导致跟踪误差增大。我们采用曲率自适应算法：
matlab复制kappa = abs(dphi/ds); % 路径曲率
Delta = Delta0 / (1 + 10*kappa);
3.3 深度控制补偿
考虑到水压对深度传感器的影响，增加压力补偿项：
matlab复制P = rho*g*abs(z);
depth_comp = P/(rho*g) + 0.1*sin(2*pi*t/10); % 包含波浪扰动模型
4. Matlab实现关键代码
4.1 主控制循环
matlab复制function [u, states] = uuv_control(t, states, path)
% 状态提取
[x,y,z,phi,theta,psi,u,v,w,p,q,r] = deal(states(1:12));
% LOS制导
[psi_d, theta_d, la_point] = los_guidance(x,y,z, path);
% PID控制
tau_phi = pid_roll(phi, 0, dt);
tau_theta = pid_pitch(theta, theta_d, dt);
tau_psi = pid_yaw(psi, psi_d, dt);
% 深度控制
f_z = pid_depth(z, path.z_des, dt);
% 推进分配
u = thrust_allocation([tau_phi; tau_theta; tau_psi; f_z]);
end
4.2 推进分配算法
考虑推进器饱和特性的伪逆分配：
matlab复制function u = thrust_allocation(tau)
B = [...]; % 推进器配置矩阵
W = diag([1, 1, 1, 0.8]); % 权重矩阵
u = pinv(B'*W*B)*B'*W*tau;
u = max(min(u, u_max), u_min); % 饱和限制
end
5. 实测问题与解决方案
5.1 海流干扰问题
现象：侧向海流导致持续位置偏差
解决：在LOS层增加积分项：
matlab复制% 修改后的LOS角度计算
psi_d = atan2(y_la - y + k_i*int_y, x_la - x + k_i*int_x);
5.2 深度振荡问题
现象：在密度跃层出现深度高频振荡
解决：增加深度变化率限制：
matlab复制theta_d = max(min(theta_d, theta_d_prev + 0.05), theta_d_prev - 0.05);
5.3 推进器耦合效应
现象：俯仰控制影响深度控制
解决：采用解耦补偿算法：
matlab复制f_z_comp = f_z - 0.3*tau_theta; % 经验补偿系数
6. 参数整定经验
6.1 LOS参数
- 前视距离基准值：Δ0 = 2.5倍艇长
- 曲率敏感系数：kκ = 10（经验值）
- 积分增益：ki = 0.05（弱积分）
6.2 PID参数
采用先内环后外环的整定顺序：
-
先整定姿态环（ϕ/θ/ψ）：
matlab复制% 滚转PID示例 Kp_roll = 1.2*(2*pi*0.8)^2*Ixx; Kd_roll = 1.2*2*0.8*(2*pi*0.8)*Ixx; -
再整定位置环（x/y/z）：
matlab复制% 深度PID示例 Kp_z = 0.8*wn_z^2/max_thrust; Ki_z = 0.5*wn_z*Kp_z;
实测技巧：在水池测试时，先用小增益确保稳定，再逐步增大。我们通常从理论值的30%开始逐步上调。
7. 扩展应用
7.1 多UUV协同
通过修改LOS参考路径实现编队：
matlab复制% 僚艇路径生成
path_follower = path_leader + [d_x; d_y; d_z];
7.2 动态避障
结合人工势场法：
matlab复制repulsive = sum(([x;y;z]-obs_pos)./norm([x;y;z]-obs_pos).^3, 2);
psi_d = psi_d + atan2(repulsive(2), repulsive(1));
7.3 能量优化
速度与前视距离自适应：
matlab复制v_opt = sqrt(2*D/(rho*Cd*A));
Delta = Delta0*(v/v_opt)^0.5;
这个方案经过我们团队在渤海海域的实际验证，在3级海况下（浪高0.8-1.2米）仍能保持跟踪误差小于艇长的5%。核心在于LOS提供了良好的前馈指令，而PID则有效抑制了外界干扰。代码中特别要注意坐标转换的顺序和推进分配的饱和处理，这是实际应用中容易出问题的环节。