欠驱动AUV轨迹跟踪与路径跟随控制算法对比

1. 项目背景与核心挑战

欠驱动自主水下航行器（AUV）是海洋探测、资源开发等领域的关键装备。与全驱动系统不同，欠驱动AUV的推进器数量少于自由度数量（通常只有主推进器和垂直/水平舵），这种结构在提升续航能力的同时，也带来了控制上的特殊挑战——无法直接控制所有方向的运动状态。

轨迹跟踪（Trajectory Tracking）要求AUV严格遵循时间-空间参数化的路径，而路径跟随（Path Following）则只关注几何路径的收敛，对时间参数不敏感。这两种控制目标在实际应用中各有优劣：前者适合时间敏感任务（如协同作业），后者更适应节能巡航等场景。

关键区别：轨迹跟踪是时变系统控制问题，路径跟随可视为时不变系统控制问题。这个根本差异导致两者在控制器设计时面临不同的稳定性分析框架。

2. 控制算法分析框架比较

2.1 基于李雅普诺夫直接法的设计

在Matlab仿真中，我们首先构建AUV的六自由度非线性动力学模型。考虑流体动力阻尼矩阵D(v)的对称正定性，采用如下能量型李雅普诺夫函数：

matlab复制% 李雅普诺夫函数示例（路径跟随）
V = 0.5*(xe'*Q*xe + psi_e^2); 
dV = xe'*Q*dxe + psi_e*dpsi_e;

其中xe为位置误差，psi_e为航向角误差。通过反步法（Backstepping）逐步处理子系统误差，最终得到推力τ和舵角δ的控制律。仿真显示该方法对参数摄动具有鲁棒性，但需要精确的动力学模型。

2.2 模型预测控制(MPC)实现

在Simulink中搭建预测控制器，核心是以下优化问题的实时求解：

matlab复制function [U_opt, cost] = MPC_Solver(X0, ref_traj)
    cvx_begin
        variable U(Nc,2) % 控制量序列
        minimize (sum_square(X(:,1:3)-ref_traj) + 0.1*sum_square(U))
        subject to
            X = predictDynamics(X0,U); % 动力学预测
            abs(U) <= [F_max; delta_max]'; % 执行器约束
    cvx_end
end

实测表明，MPC在轨迹跟踪任务中表现优异，但计算负荷较高（单步求解约15ms），需权衡预测时域长度与控制实时性。

2.3 滑模变结构控制

针对模型不确定性，设计积分型滑模面：

code复制s = e + lambda*integral(e) 

其中e为跟踪误差，lambda为调节参数。采用饱和函数sat(s/Φ)代替符号函数减轻抖振。Simulink仿真显示该方法在20%参数扰动下仍保持稳定，但舵机需承受高频切换动作。

3. 仿真平台构建关键步骤

3.1 高保真动力学建模

在Matlab中建立包含以下耦合项的动力学方程：

matlab复制function dx = auv_dynamics(t,x,u)
    % 科里奥利力矩阵
    C = [0, -m*v(3), m*v(2);
         m*v(3), 0, -m*v(1);
         -m*v(2), m*v(1), 0];
    % 流体动力阻尼（二次项主导）
    D = -diag([Xu+Xu_abs*u(1), Yv+Yv_abs*v(2), Zw+Zw_abs*v(3)]);
    dx = inv(M)*(C*v + D*v + g(eta) + tau);
end

注意事项：必须包含Munk力矩等交叉耦合项，否则水平面/垂直面运动会解耦失真。

3.2 环境扰动建模

在Simulink中添加随机海浪扰动模型：

code复制Ocean_current = J(eta)*[0.2*randn; 0.1*randn; 0]; 
Wave_force = 0.5*rho*Cd*A*(Uc - v)^2;

建议采用JONSWAP谱模拟不规则波，频谱参数根据海况等级调整。

4. 性能对比与结果分析

4.1 定量评价指标

定义以下性能指标进行横向对比：

4.2 典型场景测试结果

螺旋上升轨迹跟踪测试数据对比：

可见MPC在精度和能耗间取得较好平衡，而滑模控制在强扰动下（如添加1m/s侧向流）仍能保持0.6m内的误差。

5. 工程实现中的经验技巧

5.1 参数整定规律

通过数百次仿真总结出以下调节规律：

• 反步法中李雅普诺夫函数权重矩阵Q的对角元比值决定空间误差分配，建议z方向权重是x-y的1.5倍以抵抗浮力扰动
• MPC的预测时域Tp应大于AUV的惯性时间常数（通常Tp≈3*(M/D)）
• 滑模面参数λ与系统带宽的关系：λ ≈ 0.8ω_n，其中ω_n为期望响应频率

5.2 实时性优化

在Simulink中采用以下技巧提升运行效率：

1. 将动力学方程的求解器改为ode4（固定步长Runge-Kutta），步长取0.05s
2. 对MPC的QP求解使用预生成的C代码（通过MATLAB Coder实现）
3. 滑模控制的sign函数用连续近似：sign(x) ≈ x/(|x|+0.01)

5.3 常见故障排查

• 问题1：仿真中出现姿态角发散

  • 检查：重力与浮力矩是否平衡（应满足z_b > z_g）
  • 解决：调整压载配置或添加积分项消除静差

• 问题2：路径跟随出现"割线"现象

  • 检查：前视距离L是否合适（建议L=1.5~2倍船长）
  • 解决：引入自适应调整：L = k*U，其中U为航速

6. 进阶研究方向

在实际项目中，我们进一步探索了以下方向：

1. 基于强化学习的参数自整定：用DQN在线优化MPC的权重矩阵
2. 事件触发控制：在误差超过阈值时才更新控制量，可降低40%舵机动作频次
3. 多AUV协同路径跟随：结合虚拟领航者策略，保持编队构型

仿真代码中特别实现了动态前视距离算法：

matlab复制function L = adaptive_lookahead(U, psi_dot)
    L_min = 1.0; % 最小前视距离
    L_base = 2.5; % 基准距离
    L = max(L_min, L_base - 0.5*abs(psi_dot)); % 转弯时缩短前视距离
end

这种处理显著改善了急转弯路径的跟踪性能，实测曲率半径可减小至2倍船长。