UUV AUV 三维路径跟踪 LOS制导+滑膜控制算法 MATLAB编程实现

UUV AUV 三维路径跟踪
LOS制导+滑膜控制算法
MATLAB编程实现
六自由度MMG模型

以下文字及示例代码仅供参考
【CSDN 原创】UUV/AUV 六自由度三维路径跟踪 | LOS 制导 + 滑模控制（SMC） | MATLAB 全流程开源

关键词：UUV, AUV, 六自由度, MMG 模型, LOS 制导, 滑模控制, MATLAB, CasADi, 三维路径跟踪

一、方案亮点

1. 六自由度 MMG 非线性模型
   完整保留纵荡、横荡、垂荡、横摇、纵摇、艏摇，耦合水动力、附加质量、阻尼、恢复力，可直接替换为任意公开水动力系数。
2. LOS 三维制导
   基于“管道”思想，将三维航迹分解成水平 LOS 与垂直 LOS，实时计算期望航向角 θd、期望俯仰角 φd，实现无横滑角跟踪。
3. 终端滑模控制
   对欠驱动 4 输入（左右主推、艏艉舵、垂舵）设计三通道滑模面：
   • 纵向速度 u → 主推
   • 水平 LOS 误差 ey → 艏舵
   • 垂直 LOS 误差 ez → 垂舵
     保证有限时间收敛、鲁棒抗流。
4. 单脚本可跑
   仅需 MATLAB + CasADi（自动微分 + ipopt），无需 Simulink。

二、文件结构

UUV_3D_TRACK/
 ├─ main.m                % 一键运行
 ├─ uuv6dof.m             % 6-DOF MMG 动力学
 ├─ los3d.m               % 三维 LOS 制导
 ├─ smcCtrl.m             % 滑模控制器
 ├─ refPath.m             % 任意三维参考线生成（螺旋、折线等）
 ├─ plot3D.m              % 实时三维动画
 └─ README.pdf            % 公式推导 & 调参手册

三、核心公式

1. 六自由度动力学（MMG 形式）
   [
   \boldsymbol{M}\dot{\boldsymbol{\nu}} + \boldsymbol{C}(\boldsymbol{\nu})\boldsymbol{\nu} + \boldsymbol{D}(\boldsymbol{\nu})\boldsymbol{\nu} + \boldsymbol{g}(\boldsymbol{\eta}) = \boldsymbol{\tau} + \boldsymbol{\tau}_{\text{env}}
   ]
   其中 η=[x,y,z,ϕ,θ,ψ]′，ν=[u,v,w,p,q,r]′，τ=[X,Y,Z,K,M,N]′ 由 4 台推进器映射。

2. 三维 LOS 误差
   [
   \begin{aligned}
   e_y &= -(x-x_d)\sin\psi_p + (y-y_d)\cos\psi_p \
   e_z &= -(z-z_d)\cos\theta_p
   \end{aligned}
   ]
   期望航向角
   [
   \psi_d = \psi_p + \arctan2(-e_y, \Delta_h), \quad
   \theta_d = \theta_p + \arctan2(e_z, \Delta_v)
   ]

3. 滑模面
   对通道 i 取
   [
   s_i = \dot{e}_i + k_i |e_i|^\alpha \text{sgn}(e_i)
   ]
   控制律
   [
   \tau_i = -K_i \text{sat}(s_i/\Phi_i) - \hat{\rho}_i \text{sgn}(s_i)
   ]
   其中 sat 为边界层函数，Φ_i 为边界层厚度。

四、关键代码

1. 六自由度 MMG (uuv6dof.m)

function nu_dot = uuv6dof(t,nu,eta,tau,p)
% nu = [u;v;w;p;q;r], eta = [x;y;z;phi;theta;psi]
M   = p.M; C = p.C(nu); D = p.D(nu); G = p.G(eta);
nu_dot = M \ (tau - C*nu - D*nu - G);
end

2. 三维 LOS (los3d.m)

function [psi_d,theta_d] = los3d(eta,eta_p,Delta)
% eta_p = [x_p;y_p;z_p;psi_p;theta_p] 为路径点
e_y = -(eta(1)-eta_p(1))*sin(eta_p(6)) + (eta(2)-eta_p(2))*cos(eta_p(6));
e_z = -(eta(3)-eta_p(3))*cos(eta_p(5));
psi_d   = eta_p(6) + atan2(-e_y,Delta(1));
theta_d = eta_p(5) + atan2(e_z, Delta(2));
end

3. 滑模控制器 (smcCtrl.m)

function tau = smcCtrl(nu,eta,nu_d,eta_d,p)
% 仅控制 u, psi, theta
e_u = nu(1)-nu_d(1);
e_psi = wrapToPi(eta(6)-eta_d(6));
e_theta = wrapToPi(eta(5)-eta_d(5));

% 滑模面
s_u = e_u + p.ku*abs(e_u)^0.5*sign(e_u);
s_psi = wrapToPi(nu(6)) + p.kpsi*sign(e_psi)*abs(e_psi)^0.5;
s_theta = wrapToPi(nu(5)) + p.ktheta*sign(e_theta)*abs(e_theta)^0.5;

% 控制量
tau = zeros(6,1);
tau(1) = -p.ku_s*sat(s_u/p.Phi_u);
tau(5) = -p.ktheta_s*sat(s_theta/p.Phi_theta);
tau(6) = -p.kpsi_s*sat(s_psi/p.Phi_psi);
end

4. 主脚本 (main.m)

clc; clear; close all;
addpath(genpath(pwd));

p = loadUUVparam();        % 水动力参数
eta0 = [0;0;0;0;0;0];      % 初始位姿
nu0  = zeros(6,1);         % 初始速度

% 参考轨迹（螺旋线）
tspan = 0:0.1:60;
[xr,yr,zr,psi_p,theta_p] = refPath('spiral',tspan);

% 仿真
[tout,yout] = ode45(@(t,y) closedLoop(t,y,p,tspan,xr,yr,zr,psi_p,theta_p), ...
                    tspan, [eta0;nu0]);

plot3D(tout,yout,xr,yr,zr);   % 三维动画