【USV】无人水面艇的轨迹跟踪和碰撞避免的实时非线性模型预测控制研究（Matlab代码实现）

本文链接：https://blog.csdn.net/weixin_46039719/article/details/148862849

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

⛳️赠与读者

💥1 概述

一、USV 概述与技术挑战

1. 基本概念与任务需求

2. 关键技术挑战

二、轨迹跟踪控制方法

1. 主流算法分类

2. 非线性模型预测控制（NMPC）的优势

三、碰撞避免算法研究

1. 主流避障方法

2. COLREGS 规则集成

四、实时 NMPC 的轨迹跟踪与避障协同控制

1. 多目标控制策略

2. 实时性优化方案

五、研究展望与挑战

结论

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、数据、文章下载

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

随着对海洋活动（如环境监测、搜索和救援）的需求不断增加，对无人水面艇（USV）的研究受到了广泛关注。运动稳定和轨迹跟踪中的控制技术是USV需要解决的基本问题。此外，为了保证安全性，碰撞避免也应纳入控制问题中。本文提出了一种用于受约束的三自由度动态USV的实时非线性模型预测控制（NMPC）方案。将导航中的避障问题转化为NMPC中的变量约束问题，并通过成本函数中的权重值实现国际海上避碰规则（COLREGS）中的船舶避碰规则。利用开源的强大优化软件CasADi在有限范围内解决具有约束的优化问题，实现有限风险最小化。以带有两个电动推进模块的波浪自适应模块化USV为对象验证我们的方法，在MATLAB中模拟了包括点射击、轨迹跟踪以及遵循COLREGS中第14条规则的对头相遇情况在内的三种航行场景。结果表明，所提出的方法具有良好的控制性能和实时性要求。

一、USV 概述与技术挑战

1. 基本概念与任务需求

定义：USV 是一种通过自主航行或远程遥控执行任务的小型水面平台，具备模块化设计、多任务适配性（警戒、探测、反潜、搜救等），具有成本低、无人员伤亡、隐蔽性强等特点。
核心系统：包括平台（艇体、轮机）、任务载荷（传感器、武器）、通信系统和控制系统。
轨迹跟踪与避障需求：
- 轨迹跟踪：确保 USV 精确跟踪预设路径，需处理风浪干扰、模型不确定性。
- 碰撞避免：需满足国际避碰规则（COLREGS），应对静态障碍（礁石）和动态障碍（船舶）。

2. 关键技术挑战

非线性动力学：USV 运动受海况（风、浪、流）影响显著，模型参数易变。
实时性要求：避障决策需在毫秒级响应（如路径重规划时间 <50ms）。
多目标协同：需同时优化跟踪精度、避障安全性与能耗。

二、轨迹跟踪控制方法

1. 主流算法分类

方法类型	代表算法	特点与局限性	适用场景
几何跟踪法	视距导航（LOS）、纯追踪算法	将路径跟踪转为航向控制，计算简单但抗扰性差	环境干扰小的开阔水域
基于模型的方法	滑模控制（SMC）、模型预测控制（MPC）	显式处理约束，优化未来状态；MPC 实时性要求高	复杂约束与动态环境
智能控制方法	强化学习（RL）、神经网络（RBF）	自适应强，无需精确模型；依赖大量训练数据	模型不确定性高的场景

2. 非线性模型预测控制（NMPC）的优势

核心原理：通过滚动优化预测未来状态，求解最优控制输入，显式处理状态约束（如速度、舵角）。
适用性：
- 可融合船舶动力学模型（3-DOF），直接处理非线性项（如阻力、力矩）。
- 结合扰动观测器（ESO）提升抗干扰能力。
典型案例：
- NMPC 制导律：替代传统视线法（LOS），通过预测优化降低跟踪误差（位置偏差 <0.5m）。
- 实用 NMPC（PNMPC） ：对非线性模型线性化，减少计算量，实现实时控制。

三、碰撞避免算法研究

1. 主流避障方法

方法	技术核心	优势	局限
人工势场法（APF）	构建引力（目标点）与斥力（障碍物）场，引导 USV 绕行	直观易实现；适合静态障碍	易陷局部最优，动态避障弱
速度障碍法（VO）	预测障碍物运动轨迹，计算避障所需速度增量	符合 COLREGS 规则；实时性好	多目标避障复杂度高
深度强化学习（DRL）	结合 CNN 提取环境特征，通过奖励函数学习避障策略（如 Prioritized Experience Replay）	自适应动态环境；支持复杂决策	训练成本高，泛化性受限

2. COLREGS 规则集成

关键改进：
- 虚拟障碍线（VO-RRT） ：基于 RRT 算法加载 COLREGS 约束（如右转避让规则），避免无效路径扩展。
- 最危险障碍策略：动态筛选威胁度最高的障碍物，优先处理。
效果：实船测试表明，算法重规划时间 <50ms，符合 COLREGS 第 6、8、13-18 条规则。

四、实时 NMPC 的轨迹跟踪与避障协同控制

1. 多目标控制策略

分层架构：
1. 规划层：生成全局路径（结合 APF 或 VO-RRT 避障）。
2. 跟踪层：NMPC 接收路径参考值，滚动优化控制输入。
协同设计案例：
- ESO + APF：扩展状态观测器（ESO）估计扰动，改进 APF 斥力函数实现 COLREGS 合规避障。
- 场理论 + MPC：电场模型快速规划避障方向，MPC 优化跟踪轨迹。

2. 实时性优化方案

优化策略	技术手段	效果
减少控制步数	缩短预测时域，降低单步计算量（对比：减少步数比降频方案误差更小）	计算时间 < 控制周期，适合高机动场景
分层 MPC（HMPC）	分离规划层（PMPC）与跟踪层（TMPC），离线计算终端约束	提升 50% 计算效率，支持长时间尺度规划
并行粒子群优化（PSO）	分治策略并行求解 NMPC 优化问题，加速收敛	满足旋转倒立摆等快速动态系统实时需求
线性化近似	将非线性模型转化为凸优化问题（如电流饱和约束线性化）	计算效率提升 3 倍，保持稳定性

五、研究展望与挑战

算法融合：
- 结合深度学习与 NMPC（如 LB-NMPC），利用高斯过程处理模型不确定性。
硬件加速：
- 嵌入式 HMPC 部署，提升片上实时计算能力。
多智能体协同：
- 异构系统（UAV-USV）编队控制，需统一避障策略与通信协议。
规则适应性：
- 动态调整 COLREGS 权重，适应异常船舶行为。

结论

实时 NMPC 是 USV 轨迹跟踪与避障协同控制的核心解决方案：

轨迹跟踪：NMPC 通过滚动优化和扰动补偿，显著提升精度（位置误差 <1m）。
碰撞避免：集成 VO-RRT 和 DRL 的方法可满足 COLREGS 实时避障要求（响应时间 <50ms）。
实时优化：分层 MPC 和减少控制步数策略有效平衡计算效率与控制性能。

未来需进一步突破智能算法与嵌入式硬件的协同优化，推动 USV 在高动态海洋环境中的自主化应用。

📚2 运行结果

部分代码：

function Draw_MPC_PSship_Obstacles (t,xx,xx1,u_cl,xs,N,rob_diam,obs_x,obs_y,obs_diam)

set(0,'DefaultAxesFontName', 'Times New Roman')
set(0,'DefaultAxesFontSize', 12)

line_width = 1.5;
fontsize_labels = 14;

%--------------------------------------------------------------------------
%-----------------------Simulate robots -----------------------------------
%--------------------------------------------------------------------------
x_r_1 = [];
y_r_1 = [];



r = rob_diam/2;  % obstacle radius
ang=0:0.005:2*pi;
xp=r*cos(ang);
yp=r*sin(ang);

r = obs_diam/2;  % obstacle radius
xp_obs=r*cos(ang);
yp_obs=r*sin(ang);

figure(500)
% Animate the robot motion
%figure;%('Position',[200 200 1280 720]);
set(gcf,'PaperPositionMode','auto')
set(gcf, 'Color', 'w');
set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]);

tic
for k = 1:size(xx,2)
    h_t = 4.88; w_t=2.44; % triangle parameters
    
    x1 = xs(4); y1 = xs(5); th1 = xs(6);
    x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)];
    y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)];
    fill(x1_tri, y1_tri, 'g'); % plot reference state
    hold on;
    x1 = xx(4,k,1); y1 = xx(5,k,1); th1 = xx(6,k,1);
    x_r_1 = [x_r_1 x1];
    y_r_1 = [y_r_1 y1];
    x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)];
    y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)];
    plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory
    if k < size(xx,2) % plot prediction
        plot(xx1(1:N,4,k),xx1(1:N,5,k),'r--*')
        for j = 2:N+1
            plot(xx1(j,4,k)+xp,xx1(j,5,k)+yp,'--r'); % plot robot circle
        end
    end
    
    fill(x1_tri, y1_tri, 'r'); % plot robot position
    
    plot(x1+xp,y1+yp,'--r'); % plot robot circle
    
    plot(obs_x+xp_obs,obs_y+yp_obs,'--b'); % plot robot circle    
    
    hold off
    %figure(500)
    ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels)
    xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels)
    axis([0 70 0 70])
    pause(0.0)
    box on;
    grid on
    %aviobj = addframe(aviobj,gcf);
    drawnow
    % for video generation
    F(k) = getframe(gcf); % to get the current frame
end
toc
% close(gcf)
%viobj = close(aviobj)
% video = VideoWriter('exp.avi','Uncompressed AVI');
% 
%  video = VideoWriter('exp.avi','Motion JPEG AVI');
%  video.FrameRate = 5;  % (frames per second) this number depends on the sampling time and the number of frames you have
%  open(video)
%  writeVideo(video,F)
%  close (video)

U_pre = xx(1,2:end);V_pre = xx(2,2:end); R_pre = xx(3,2:end);
figure
subplot(511)
plot(t',U_pre,'linewidth',1.5);xlabel('time (s)');ylabel('u (m/s)');grid on
subplot(512)
plot(t',V_pre,'linewidth',1.5);xlabel('time (s)');ylabel('v (m/s)');grid on
subplot(513)
plot(t',R_pre,'linewidth',1.5);xlabel('time (s)');ylabel('r (rad/s)');grid on
subplot(514)
stairs(t,u_cl(:,1),'r','linewidth',1.5); xlabel('time (s)');ylabel('T_p (N)');grid on
subplot(515)
stairs(t,u_cl(:,2),'r','linewidth',1.5); xlabel('time (s)');ylabel('T_s (N)');grid on