顶刊复现，基于Lyapunov的模型预测控制MPC方法，用于控制水下机器人AUV的路径跟踪问题trajectory tracking 具体的方法和建模过程可以参考文献 本代码包括水下机器人的fossen动力学模型，matlab的优化算法求解器，还包括非线性反步法backstepping 的对比代码非常划算，两种对比都有

水下机器人轨迹跟踪这活儿不好干，水流扰动、模型非线性、执行器饱和这些破事一箩筐。最近复现了篇用Lyapunov-Based MPC搞AUV控制的顶刊方案，顺手把反步法对比也撸出来了，实测比传统方法稳得多。

先看Fossen动力学模型怎么建的。这个经典模型考虑流体动力学的耦合效应，状态向量取η=[x,y,z,φ,θ,ψ]和ν=[u,v,w,p,q,r]，动力学方程写成矩阵形式：

% Fossen模型核心代码片段
function dx = auv_model(x, tau)
    M = diag([200 200 200 80 80 80]); % 惯性矩阵
    D = diag([70 70 70 30 30 30]);    % 阻尼矩阵
    J = euler2jac(x(4:6));            % 欧拉角转换矩阵
    dx(1:6,1) = J*x(7:12);
    dx(7:12,1) = inv(M)*(tau - D*x(7:12) - coriolis(x)); 
end

这里关键在科氏力计算coriolis()函数，需要按文献里的公式处理交叉耦合项。有个坑要注意：当俯仰角θ接近±90度时欧拉角会出现奇点，实际部署得用四元数，但仿真时用这个够用了。

重点来了——Lyapunov-Based MPC的设计。不同于常规MPC直接用跟踪误差作代价函数，这里把Lyapunov函数V=0.5e^TP*e作为约束条件，要求V(k+1) ≤ ρV(k)。优化问题长这样：

% MPC优化核心逻辑
for k = 1:N
    cost = cost + (x_ref(:,k)-x_pred(:,k))'*Q*(x_ref(:,k)-x_pred(:,k));
    cost = cost + u_pred(:,k)'*R*u_pred(:,k); 
    constraints = [constraints, V_pred(k+1) <= rho*V_pred(k)]; % Lyapunov衰减约束
end
options = optimoptions('fmincon','Algorithm','sqp');
[u_opt,~,exitflag] = fmincon(@(u) mpc_cost(u), u_guess,[],[],[],[],lb,ub,@(u) mpc_constraints(u),options);

这里用了fmincon求解，但实测用YALMIP建模后用IPOPT更快，尤其是当预测时域N超过20步时。代价函数里的Q矩阵别全设成对角阵，参考位姿和速度的权重建议按1:0.3分配。

反步法对比方案就暴力多了，直接级联设计虚拟控制量：

% 反步法推力分配示例
alpha1 = -K1*(eta - eta_d) + dot_eta_d;
alpha2 = -K2*(nu - alpha1) + dalpha1_dt;
tau = M*alpha2 + D*nu + coriolis_term;

这方法在低扰动下还行，但遇到强海流直接崩。测试时故意在t=15s加了个2m/s的横向扰动，MPC方案能在8秒内收敛，反步法直接跑出画面外了。

代码包里还有几个实用函数：

1. 轨迹生成器用Clothoid曲线做螺旋路径
2. 可视化模块实时显示AUV位姿和跟踪误差
3. 执行器饱和模型（别小看这个，没做饱和限制的MPC会算出离谱的控制量）

跑仿真建议先用ODE45，但实时测试时换定步长欧拉法。采样周期别超过0.2秒，否则优化器来不及算完。实际部署时可以把线性化后的QP问题预生成代码，能快3倍以上。