手把手玩转UUV三维路径跟踪

原创于 2025-12-16 17:45:00 发布 · 200 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#基带工程

UUV三维路径跟踪，LOS制导+PID控制实现模型参数可调，期望轨迹可调，速度可调 MATLAB编程实现想要可直接联系~~~

水下无人潜器（UUV）的路径跟踪听着高大上，但其实只要抓住LOS制导+PID这对黄金组合，配合MATLAB的矩阵操作，半天就能搭出可调参数的仿真原型。直接上干货！

先搞清LOS制导的核心

LOS（Line of Sight）制导说白了就是让UUV始终朝着"视线方向"前进。三维情况下，我们把期望轨迹拆解成水平面的路径跟踪和垂直面的深度控制。核心公式就这两行：

% 水平面LOS方位角计算
psi_d = atan2(yd - y, xd - x) + atan2(K_los*e, 1);
% 垂直面LOS俯仰角计算
theta_d = atan2(zd - z, sqrt((xd - x)^2 + (yd - y)^2)) + atan2(K_los_z*e_z, 1);

这里的K_los是前视距离系数，越大跟踪越激进但可能振荡。调试时建议从0.5开始逐步调整。

PID控制要玩点花样

传统PID直接怼误差容易翻车，咱们把LOS输出的期望角速度作为PID输入：

% 水平面角速度控制
omega_ref = Kp*(psi_d - psi) + Ki*integral_error + Kd*(psi_dot_prev - psi_dot);
% 深度控制推力
Fz = Kp_z*(z_des - z) + Ki_z*integral_z + Kd_z*(z_dot_prev - z_dot);

注意这里用了角速度的微分项而不是直接位置微分，实测稳定性提升明显。PID参数建议先用Ziegler-Nichols法粗调，再根据响应微调。

MATLAB实现的关键技巧

参数结构体封装所有可调参数，调试时改一个文件就行：

params.speed = 1.5;       % 巡航速度
params.K_los = 0.8;       % LOS前视系数
params.PID = [2.5, 0.1, 1.2]; % [Kp, Ki, Kd]

轨迹生成器做成独立函数，支持螺旋线、正弦等多种轨迹：

function [x_d, y_d, z_d] = gen_trajectory(t, type)
    if type == "helix"
        x_d = 10*cos(0.1*t);
        y_d = 10*sin(0.1*t);
        z_d = 0.2*t;
    end
end

主循环架构要包含动力学模型，这里简化成二阶积分：

for k = 1:N
    [u_ref, v_ref, w_ref] = guidance_system(); % LOS制导
    [tau_u, tau_v, tau_w] = pid_controller();   % PID控制
    
    % 动力学更新
    vel = vel + (tau - D*vel)*dt;
    pos = pos + vel*dt;
end

调试时重点关注这些信号：