基于MPC的无人船自主控制算法设计与实现

1. 无人船（USV）自主控制研究概述

无人水面艇（USV）作为一种无需人工直接操控的水面机器人，近年来在海洋勘测、环境监测、搜救和军事防御等领域展现出巨大潜力。与传统有人船相比，USV具有显著优势：它能够在危险环境中执行任务（如反海盗、水雷清除），采用锂离子电池或太阳能供电实现长时间作业，并且通过预设程序或远程控制实现自主/半自主航行。

在USV的诸多技术挑战中，自主控制系统的设计尤为关键。传统PID控制虽然实现简单，但在复杂海洋环境中往往表现不佳。模型预测控制（MPC）因其能够显式处理系统约束和环境干扰，成为USV高精度控制的有力解决方案。本次研究聚焦于基于MPC的USV自主控制算法开发，特别针对航点跟踪任务进行了优化设计。

2. MPC控制算法设计与实现

2.1 算法核心思路

MPC算法的核心在于三个关键环节：预测模型、滚动优化和反馈校正。在USV控制场景中：

1. 预测模型：采用简化的USV动力学模型，包括质量m=37kg、电机与质心距离D=0.7m、转动惯量I=0.1kg·m²等参数
2. 滚动优化：在每个控制周期（Te=0.1s）求解有限时域（ny=30步）的最优控制问题
3. 反馈校正：通过状态估计补偿模型误差和环境干扰

初始方案尝试在全局坐标系中直接优化USV位置，但由于坐标系转换引入的非线性导致优化困难。改进后的策略转为在船体坐标系中优化角度和前向速度，显著提升了计算效率和跟踪性能。

2.2 状态空间建模

USV的状态空间模型是MPC实现的基础。我们定义了两种状态表示：

1. MPC核心模型：5维状态向量a=[θ̈ θ̇ θ ẋ U]ᵀ，其中θ为航向角，U为前向速度
2. 参考生成模型：9维扩展状态x=[x_global y_global θ θ̇ θ̈ ẋ ẏ Ux Uy]ᵀ，用于航点跟踪和死航行计算

系统矩阵构建如下：

matlab复制A = [1 0 0 0 0;       % 状态矩阵
     Te 1 0 0 0;
     0 Te 1 0 0;
     0 0 0 1 Te;
     0 0 0 -k/m 1];
B = [D/(2*I) -D/(2*I); % 输入矩阵
     0 0; 
     0 0; 
     0 0; 
     1/m 1/m];

其中k=0.1为粘性系数，反映水阻力影响。

2.3 干扰建模与处理

海洋环境中的干扰（如风浪、洋流）通过Dx=-0.02、Dy=0.01参数化，方向角α=atan2(Dy,Dx)。干扰向量Dk整合到状态更新中：

matlab复制Dk = [0 0 0 0 0 0 ...
      (1/m)*Dx*cos(α-θ) 0 (1/m)*Dy*sin(α-θ)]';

这种显式干扰建模使MPC能够预测并补偿环境影响，显著提升鲁棒性。

3. 航点跟踪策略实现

3.1 智能参考生成

航点跟踪的核心是生成合适的航向角参考。算法动态计算当前航点方向：

matlab复制goal_angle = angle(complex(r_list(obj,1)-x(1), r_list(obj,2)-x(2)));

并采用最短路径原则处理角度环绕问题：

matlab复制if abs(goal_angle - x(3)) > pi
    if x(3) < 0
        goal_angle = x(3) - (2*pi - dist);
    else
        goal_angle = x(3) + (2*pi - dist);
    end
end

当USV进入航点0.5米范围内时，自动切换至下一航点：

matlab复制if(x(1)>r_list(obj,1)-0.5 && x(1)<r_list(obj,1)+0.5 && ...
   x(2)>r_list(obj,2)-0.5 && x(2)<r_list(obj,2)+0.5)
    obj = mod(obj,7) + 1;  % 循环切换航点
end

3.2 优化问题构建

MPC将控制问题转化为二次规划(QP)形式：

code复制min ΔUᵀHΔU + 2fᵀΔU
s.t. Cmin ≤ CΔU ≤ Cmax

其中关键矩阵通过以下函数计算：

matlab复制Qbar = computeQbar(C_tilde, gainQ, gainS, ny);
Rbar = computeRbar(gainR, nu);
Cbar = computeCbar(A_tilde, B_tilde, ny, nu);
Hbar = computeHbar(Cbar, Qbar, Rbar);

状态约束设置为：

matlab复制Cmin = [-100 -100 -2*pi 0 -10000 -1000*k/2 -1000*k/2]';
Cmax = [100 100 2*pi V_cruise 10000 1000*k/2 1000*k/2]';

4. 仿真结果与分析

4.1 无干扰场景测试

在Tfinal=150s的仿真中，USV成功跟踪10个航点组成的矩形路径（坐标：[2,2]->[2,22]->[7,22]->...->[22,22]）。关键观察：

1. 航点切换平滑，最大跟踪误差<0.5m
2. 航向角在t≈100s出现异常漂移，可能与离散化误差积累有关
3. 前向速度存在稳态误差，建议增加积分环节

4.2 干扰场景测试

引入[Dx,Dy]=[-0.02,0.01]的恒定干扰后：

1. 航点跟踪仍能完成，验证了算法鲁棒性
2. 横向速度（y轴）受干扰明显，因控制目标未直接优化该变量
3. 轨迹出现"绕行"现象，这是MPC在干扰下寻找可行解的结果

5. 关键实现细节与技巧

5.1 MATLAB代码优化

1. 预分配内存：仿真前初始化数据向量避免动态扩展

matlab复制x = zeros(9, ceil(Tfinal/Te)); % 状态向量
u = zeros(2, ceil(Tfinal/Te)); % 输入向量

2. QP求解配置：关闭quadprog输出保持命令行整洁

matlab复制options = optimoptions('quadprog', 'Display', 'off');

5.2 参数整定经验

1. 预测时域选择：ny=30（3s）平衡了计算负担与前瞻性
2. 控制时域选择：nu=2避免过激的控制动作
3. 权重调整：角度误差权重设为速度误差的2倍，体现航向优先

5.3 常见问题排查

1. QP不可行：检查约束是否过紧，特别是速度上限V_cruise
2. 振荡现象：适当增大输入变化惩罚gainR
3. 计算延迟：可减少ny或采用热启动技巧

6. 扩展方向与改进建议

1. 模型精度提升：引入辨识实验获取准确的I、k参数
2. 干扰观测器：设计NDO估计并补偿时变干扰
3. 积分环节：增加状态变量消除速度稳态误差
4. 硬件加速：使用代码生成技术部署到实时处理器

实际测试中发现，电机饱和限制(k/2)对性能影响显著。建议在实际系统中选用扭矩余量更大的推进器，或设计输入分配策略处理饱和情况。