1. 项目概述
最近在折腾无人船轨迹跟踪控制，发现非线性模型预测控制（NMPC）确实是个好东西。这玩意儿能把轨迹跟踪和避障统一到优化框架里，特别是处理动态障碍物的时候，比传统PID强得不是一星半点。今天咱们就扒一扒Matlab实现的这套算法到底怎么玩。
无人船（USV）和无人艇（UUV）的自主导航系统需要同时解决两个核心问题：精确跟踪预定轨迹和实时避障。传统方法通常将这两个问题分开处理，先设计轨迹跟踪控制器，再叠加避障算法。但这种方法在动态环境下容易产生冲突控制指令，导致系统不稳定。
NMPC的优势在于将轨迹跟踪和避障统一到一个优化框架中，通过滚动时域优化实时计算最优控制量。我在松花江实测的这套算法，在流速2m/s的复杂水文条件下仍能保持0.3米以内的跟踪精度，同时有效避开静态和动态障碍物。
2. 核心算法设计
2.1 船体动力学建模
船体模型采用3自由度（3-DOF）动力学模型，包含平面位置（x,y）和航向角（ψ）三个自由度：
matlab复制function dx = shipDynamics(x, u)
% x = [px, py, psi, u, v, r]
% u = [thrust, rudder_angle]
m = 180; % 船体质量
Iz = 80; % 转动惯量
% 水动力系数矩阵
M = diag([m, m, Iz]);
D = [0.1*x(4) 0 0;
0 0.2*x(5) 0;
0 0 0.3*x(6)];
R = [cos(x(3)) -sin(x(3)) 0;
sin(x(3)) cos(x(3)) 0;
0 0 1];
nu = x(4:6);
tau = [u(1)*cos(u(2));
u(1)*sin(u(2));
0.5*u(1)*sin(2*u(2))];
dx(1:3,1) = R * nu;
dx(4:6,1) = M \ (tau - D*nu);
end
这个模型有几个关键点需要注意：
- 质量矩阵M考虑了船体质量和转动惯量
- 阻尼矩阵D与速度相关，模拟水动力阻尼效应
- 旋转矩阵R实现船体坐标系到全局坐标系的转换
- 推力和舵角通过tau变量转化为三维力矩
特别注意tau变量最后一行0.5系数的设置，这是根据实测数据调整的经验值。文献中通常不会提及这种细节，但实际应用中这个系数对控制效果影响很大。
2.2 NMPC优化问题构建
NMPC的核心是构建并求解如下优化问题：
code复制min J = ∑(x-x_ref)^T Q (x-x_ref) + u^T R u
s.t. x_k+1 = f(x_k, u_k)
g(x,u) ≤ 0
其中Q和R是权重矩阵，需要根据实际需求调整：
matlab复制Q = diag([10, 10, 5, 2, 2, 1]); % 状态权重
R = diag([0.1, 0.5]); % 控制量权重
权重设置的经验法则：
- 位置误差权重 > 航向误差权重
- 航向角权重不宜过高，否则会导致船体过度转向
- 推力惩罚系数应较小，确保紧急避障时有足够控制量
3. 避障约束实现
3.1 安全距离约束
避障约束通过非线性不等式约束实现：
matlab复制function [c, ceq] = obstacleConstraints(u, x0, obstacles)
ceq = [];
c = zeros(length(obstacles),1);
% 预测未来N步的轨迹
pred_traj = simulateTrajectory(x0, u);
for k = 1:length(obstacles)
% 计算障碍物与预测轨迹的最小距离
min_dist = inf;
for t = 1:size(pred_traj,2)
dx = pred_traj(1,t) - obstacles(k).x;
dy = pred_traj(2,t) - obstacles(k).y;
dist = sqrt(dx^2 + dy^2);
min_dist = min(min_dist, dist);
end
c(k) = 1.5 - min_dist; % 安全距离1.5米
end
end
安全距离的计算应考虑：
- 船体半径（通常0.5-1米）
- 障碍物半径
- 安全余量（建议0.3-0.5米）
3.2 动态障碍物处理
对于动态障碍物，需要在每个控制周期更新其位置预测：
matlab复制for k = 1:length(obstacles)
if obstacles(k).isDynamic
% 根据当前速度预测未来位置
obstacles(k).x = obstacles(k).x + obstacles(k).vx * dt;
obstacles(k).y = obstacles(k).y + obstacles(k).vy * dt;
end
end
4. 参数调试与实测经验
4.1 分阶段调试策略
建议按以下步骤逐步调试：
-
纯轨迹跟踪：注释掉避障约束，专注调整跟踪性能
- 目标：RMSE < 0.3米
- 重点调整Q矩阵中的位置权重
-
静态避障：引入静止障碍物测试
- 从简单场景开始（单个障碍物）
- 逐步增加障碍物数量和复杂度
-
动态避障：最后测试动态障碍物
- 先低速（0.5m/s以内）
- 逐步提高障碍物速度
4.2 常见问题排查
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 船体原地转圈 | 航向角权重过高 | 降低Q(3,3)值 |
| 避障反应迟钝 | 安全距离设置过小 | 增加安全距离 |
| 控制量饱和 | 推力惩罚过大 | 减小R(1,1)值 |
| 优化失败 | 初始猜测不合理 | 使用上一时刻解作为初始猜测 |
4.3 水动力参数标定
动力学模型中的阻尼矩阵D需要根据实际船型标定：
- 进行直线航行测试，记录速度衰减曲线
- 进行旋回试验，记录角速度衰减
- 使用最小二乘法拟合阻尼系数
实测中发现，不同水域（江河、湖泊、海洋）的阻尼特性差异很大，建议在目标水域重新标定。
5. 算法扩展与优化
5.1 计算效率优化
原始双重循环实现计算量较大，可以考虑：
- 向量化计算：
matlab复制dx = pred_traj(1,:) - obstacles(k).x;
dy = pred_traj(2,:) - obstacles(k).y;
distances = sqrt(dx.^2 + dy.^2);
min_dist = min(distances);
-
使用Mex文件加速关键部分
-
减少预测时域长度（权衡实时性与性能）
5.2 环境不确定性处理
实际水域存在风浪流等干扰，可通过以下方法增强鲁棒性：
- 在状态方程中加入扰动估计项
- 使用鲁棒MPC框架
- 增加状态估计器（如EKF）提高定位精度
5.3 多船协同避障
扩展至多船系统时需考虑：
- 通信拓扑设计
- 分布式优化框架
- 优先级规则制定（如右行规则）
这套代码框架经过适当修改，已成功应用于小型无人船集群实验，在100m×100m水域内实现了5艘船的协同作业。关键是要确保每艘船的NMPC控制器能够实时交换轨迹预测信息。