文献复现基于非线性模型预测控制NMPC的无人船，无人艇的轨迹跟踪控制和障碍物避碰 该算法包含Matlab编写的非线性模型预测控制Nonlinear model predictive control 的无人船轨迹跟踪和障碍物避碰算法trajectory tracking and collision avoidance ，有详细的注释和参考文献。 附使用说明。
最近在折腾无人船轨迹跟踪控制，发现非线性模型预测控制（NMPC）确实是个好东西。这玩意儿能把轨迹跟踪和避障统一到优化框架里，特别是处理动态障碍物的时候，比传统PID强得不是一星半点。今天咱们就扒一扒Matlab实现的这套算法到底怎么玩。
先看核心结构——船体模型用的是3自由度动力学模型：
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这里最妙的是把推力和舵角转化为三维力矩的操作（tau变量那块），直接规避了传统方法需要单独设计航向控制器的麻烦。注意看最后一行力矩的0.5系数，这是根据实验数据调整的经验值，文献里可不会告诉你这种细节。
避障约束的实现更有意思。咱们在NMPC的优化问题里直接塞了个安全距离约束：
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这里用了双重循环来预测轨迹与障碍物的最小距离，虽然计算量大了点，但比凸近似的方法更直观。注意安全距离设置要大于船体半径+障碍物半径+安全余量，别傻乎乎直接抄1.5这个数。
实际调试时发现权重矩阵的选取比算法本身还重要。建议先用默认参数跑通，然后重点调整这几个参数：
Q = diag([10, 10, 5, 2, 2, 1]); % 状态权重 R = diag([0.1, 0.5]); % 控制量权重航向角（第三位）的权重别设太高，不然船体会疯狂扭屁股。推力惩罚系数设小点，让控制器更愿意输出大推力来紧急避障。
最后给个使用忠告：一定要先关掉避障功能单独调跟踪！见过太多新手一上来就开全功能，结果船在原地转圈还说算法不work。建议运行步骤：
- 注释掉障碍物约束
- 调跟踪效果直到RMSE小于0.3米
- 逐步增加静止障碍物
- 最后上动态障碍物
这套代码在松花江实测过，对付流速2m/s的情况没问题。不过注意动力学模型里的D矩阵系数得根据实际船型重新标定，别拿我的参数去跑海监船，翻船了别找我。
参考文献直接去看Primer论文（见代码头文件），但真正有用的参数设置全在渔民口口相传的经验里——这话也就私下说说，写论文时可别这么搞。
